clear all
clf
clc

%sound speed
c = 0.300 ; % meters/milliSsec
 
%two microphones deltaM apart initially along y-axis
deltaEar = .018 ; %meters
leftEarX = 0;
rightEarX = 0;
leftEarY = deltaEar/2;
rightEarY = - deltaEar/2;
delayEar = deltaEar/c ; %milliSeconds

%the center of the head -- position and angle wrt x-axis
headX = 0;
headY = 0;
headA = 0;
 
%initial source distance (to center of head)
%source does not move
sourceDist = .3; %meter
sourceAngle = .3 ; %radians
sourceX = sourceDist * cos(sourceAngle);
sourceY = sourceDist * sin(sourceAngle);

%=======MOTION PARAMETERS============================================
%Cricket default motion is slow forward
speed = .001 ; % meter/mSec
 
%amount of turning per spike in a motor neuron output
deltaA = .02 ; %radian
%============================================================ 

%============== NETWORK INITIALISATIONS =============================
%time step 
dt = delayEar ; % mSec
tEnd = 2 * sourceDist / speed ; %mSec
time = 0:dt:tEnd;

%Number of neurons
nNeuron = 4;

%membrane parameters
tMemb = 3*ones(1,nNeuron);  %mSec time constant
rMemb = 40*ones(1,nNeuron); %memb resistance
cMemb = tMemb./rMemb; %memb capacitance
vRest = 0;  %millivolts resting potential

reffactor = 2; % ms length of refractory period
tRef = reffactor * ones(1,nNeuron);
spikeTime = -tRef; %time of occurence of last spike
newdt = zeros(1,nNeuron); %part of timestep resulting from interpolation

vThresh = [16 16 10 10]; %threshold voltage
vRestore = [10 10 vRest vRest]; % voltage-level to which a neuron decays after a spike

tAdapt = 52*ones(1,nNeuron); %mSec time constant of slow recovery after an AP 
Ginc = .003/dt*ones(1,nNeuron);  %microSiemans: Instant increase in gAdapt when a spike occurs 
                                %dependent of dt, but in fact constant (as it should be), since dt is constant

% synaptic connection matrix    
exc = 16;
inh = 7;                            
weight =    [0   0    exc  -inh;...
             0   0   -inh   exc; ...
             0   0     0     0; ...
             0   0     0     0] ;
     
% These 2 following parameters are only used when synaptic weights are variable during run
startWeight = weight; % startweight and also maximum weight
minWeight = startWeight * 0.1; % minimum weight
     
%initialize state variables
vMemb = zeros(1,nNeuron);
spike = zeros(1,nNeuron);
gAdapt = zeros(1,nNeuron);

tindex = 1; %pointer for output arrays

wDecaySpeed = 0; % the amount of weight that is lost, each time a spike goes through a synapse
wRepairSpeed = 0; % the amount of weight that is restored, each time no spike goes through a synapse

%initialize plotting variables
Vout = zeros(length(time),nNeuron) ;  
Aout = zeros(length(time),nNeuron) ;
Sout = zeros(length(time),nNeuron) ;
% Wout is only used when synaptic weights are variable
%Wout = zeros(length(time),nNeuron) ; Weights 1-3, 1-4, 2-3 and 2-4, the only weights different from 0
%==================================================================


%Song structure from Lund paper
%Carrier frequency
freq = 4.700 ; % cycles/mSec
burstDur = 20 ; % mSec
burstInterval = 40; % mSec
%dt intervals when song is actually playing
burstGate = mod(time/dt,burstInterval/dt) < (burstDur/dt);
sourceSignal = burstGate .* sin(2*pi*time*freq);

%time constant of input lowpass filter
%and the one-pole filter coefficient
%pick tau to be longer than carrier cycle, but shorter than
%neuron dynamics
tau = 1 ; %mSec
alpha = exp(-dt/tau);
 
%init the input currents
Iright = 0;
Ileft = 0;
%init the delayed intraaural signals
LsignalPrevious = 0;
RsignalPrevious = 0;

% Variables needed to check end of program
mindist = sourceDist; % the closest point reached so far
finished = 0;


for t=time
    
    %distance to each ear
    Rright = sqrt( (sourceX-rightEarX)^2 + (sourceY-rightEarY)^2 );
    Rleft = sqrt( (sourceX-leftEarX)^2 + (sourceY-leftEarY)^2 );
    % and the corresponding propagation times
    Tright = Rright / c ;
    Tleft = Rleft / c ;
    
    %sound delayed by propagation times and 1/r**2
    if t < Tright
        Rsignal = 0;
    else
        Rsignal = sourceSignal(ceil((t-Tright)/dt))/(Rright^2); 
    end
    %
    if t < Tleft
        Lsignal = 0;
    else
        Lsignal = sourceSignal(ceil((t-Tleft)/dt))/(Rleft^2); 
    end
     
    %make currents proportional to absolute value of the signal and
    %smooth them with a 1-pole filter
    scalefactor = 1;
    Iright = alpha*Iright + scalefactor*(1-alpha)* abs(Rsignal - RsignalPrevious);
    Ileft  = alpha*Ileft  + scalefactor*(1-alpha)* abs(Lsignal - LsignalPrevious);
    
    %remember the current for the next aural time delay
    LsignalPrevious = Lsignal;
    RsignalPrevious = Rsignal;
    
    
    %====== NETWORK CODE ====================================================
     
    Is = [Ileft Iright 0 0]; % synaptic input
    Isynaps =  spike * weight; %decaying synaptic currents
    I = Isynaps +  Is; % total input
    
        %add to plotting variables
    Vout(tindex,:) = vMemb;  
    Aout(tindex,:) = gAdapt;
    Sout(tindex,:) = spike;
%    Wout(tindex,:) = [weight(1,3:4) weight(2,3:4)]; % the only weights that will differ from zero
      
    % Changes to gAdapt and vMemb
    adaptchange = - dt * gAdapt ./ tAdapt;
    membchange = (dt * ((I ./ cMemb - vMemb .* (1 + rMemb .* gAdapt) ./ tMemb))) .* ((t - spikeTime) > tRef);
    
    for i=1:nNeuron
        if t == spikeTime(i) + tRef(i)
            % If this is the timestep at which the refractory period ends, than the vMemb will have changed before it,
            % due to the interpolation, due to the exact time of the spike (although t-spikeTime < tRef):
            membchange(i) = newdt(i) * (I(i) / cMemb(i) - vMemb(i) * (1 + rMemb(i) * gAdapt(i)) / tMemb(i));
        end
    end
    
    % Generate the spike
    spike = (vMemb + membchange >= vThresh);
    
    for i=1:nNeuron
        if spike(i)
            % and its time of occurence
            spikeTime(i) = t;
            
            % TIME CALCULATIONS
            % Calculate more exact time of spiking through linear interpolation
            actualtime = dt / membchange(i) * (vThresh(i) - vMemb(i)) + t - dt;
            % Calculate how much of the dt-time-interval remains after the spiketime
            % This is the interval during which vMemb will already rise at time changeTime
            newdt(i) = (t - actualtime);
            
            % VALUES AT ACTUAL SPIKETIME
            % Calculate adaption at time of the spike
            gAdapt(i) = gAdapt(i) + adaptchange(i) / dt * (dt - newdt(i));
            % At the actual time of spike, vMemb becomes vRest
            vMemb(i) = vRest;
            
            % Changes to gAdapt and vMemb in shortened interval
            adaptchange(i) = - newdt(i) * gAdapt(i) / tAdapt(i) + Ginc(i);
            % This will always be zero, if tRef > dt
            membchange(i) = (newdt(i) * (I(i) / cMemb(i) - vMemb(i) * (1 + rMemb(i) * gAdapt(i)) / tMemb(i))) * ((t - spikeTime(i)) > tRef(i));
        end
    end
    
    % Update gAdapt and vMemb
    gAdapt = gAdapt + adaptchange;
    vMemb = vMemb + membchange;
    
%    for i=1:nNeuron
%        if spike(i)
            % The synapse-weight changes for the next iteration...if dt was included, this would be:
            % dw = w * wrepairspeed * (time before spike) - w * wdecayspeed * (time after spike)
            % without dt, this becomes simply:
%            k1w(i,:) = - weight(i,:) * wDecaySpeed;
%            k2w(i,:) = - (weight(i,:) + 0.5*k1w(i,:)) * wDecaySpeed;
%        else
%            k1w(i,:) =  weight(i,:) * wRepairSpeed;
%            k2w(i,:) = (weight(i,:) + 0.5*k1w(i,:)) * wRepairSpeed;
%        end
%    end
%    weight = sign(weight) .* min(abs(startWeight),max(abs(minWeight),abs(weight + k2w))); % keep the weight between certain limits
    
    %update time index
    Tout(tindex) = t;
    tindex = tindex+1;

    %==================================================================
    
    if spike(3)
        headA = headA + deltaA;
    end
    if spike(4)
        headA = headA - deltaA;
    end
     
    %update head center position
    headX = headX + speed*dt*cos(headA) ;
    headY = headY + speed*dt*sin(headA) ;
    
    %update the ear positions
    leftEarX = headX + deltaEar/2 * cos(headA + 0.5*pi) ;
    rightEarX = headX + deltaEar/2 * cos(headA + 1.5*pi) ;
    leftEarY = headY + deltaEar/2 * sin(headA + 0.5*pi) ;
    rightEarY = headY + deltaEar/2 * sin(headA + 1.5*pi) ;

    % Plot moving cricket
    cla
    plot([leftEarX headX rightEarX], [leftEarY headY rightEarY],'-o')
    hold on
    plot(sourceX, sourceY, 'rx')
    set(gca,'xlim', [0 .5], 'ylim', [-.05 .2] )
    drawnow

    if ((sqrt((leftEarX-sourceX)^2 + (leftEarY-sourceY)^2) < deltaEar/2) | ...
            (sqrt((rightEarX-sourceX)^2 + (rightEarY-sourceY)^2) < deltaEar/2) | ...
            (sqrt((headX-sourceX)^2 + (headY-sourceY)^2) < deltaEar/2)) ...
         & ~finished
        % When either part of the cricket is close enough to the sound source 
        % (= "has reached it"), save the present time, as returnvalue.
        result = t;
        
        % Once this has happened, the returnvalue mustn't be changed any more.
        % The computations must continue, however, to be able to do the plotting at the end of the program
        finished = 1;
        
    elseif (sqrt((headX-sourceX)^2 + (headY-sourceY)^2) < mindist) & ~finished
        % If the soundsource isn't reached yet, always remember the closest point the cricket has come.
        mindist = sqrt((headX-sourceX)^2 + (headY-sourceY)^2);
    end
end

% Plot behavior of all neurons
neuronNames = ['ANL'; 'ANR'; 'MNL'; 'MNR'];
for i=1:nNeuron
figure(i+1)
clf
np=3;

subplot(np,1,1)
plot(Tout,Vout(:,i))
title(['\bf',neuronNames(i,:)])
set(gca,'ylim',[-20 20])
ylabel('V')

subplot(np,1,2)
plot(Tout,Aout(:,i))
ylabel('Gadapt')

subplot(np,1,3)
plot(Tout,Sout(:,i))
ylabel('Spikes')
set(gca,'ylim',[-.2 1.5])
end

if ~finished
    % When the cricket never reached the sound source, the returned result is the minimal
    % distance * 1e6. This way the result will always be worse then the time in any 
    % successfull trial (in which the sound source was reached).
    result = 1e6 * mindist;
end