%Function fitPNP simulates the trajectory followed by a bird pursuing a lure,
%where the bird's steering is commanded by a mixed delayed proportional 
%navigation-pursuit (PNP) guidance law for delay tau and guidance constants 
%N and K, assuming flight at the speed given in the variable birdSpeed. The 
%initial conditions for the simulation are set by the bird's initial position 
%and velocity, and the lure's position and velocity are taken as given.
%
%This function takes the following variables as arguments:
%
    %birdPos    bird's measured 3D position  (size: n-by-3)
    %birdVel    bird's measured 3D velocity  (size: n-by-3)
    %birdSpeed  bird's measured groundspeed  (size: n-by-1)
    %lurePos    lure's measured 3D position  (size: n-by-3)
    %lureVel    lure's measured 3D velocity  (size: n-by-3)
%    
%These variables are all measured in SI units in an inertial frame of reference, 
%and are sampled at a rate 1/stepSize, where the data are assumed to have been 
%upsampled such that stepSize is appropriately small to serve as the integration
%step size for the numerical integration. For 2D analysis, simply set third column
%of all position and velocity variables to zero.
%
%This function takes the following parameters as arguments:
%
    %stepSize   sampling interval of the variables                  (units: s)
    %tauMax     maximum time delay tau to be fitted                 (units: stepSize)
    %N          guidance constant of PN component of guidance law   (units: dimensionless)
    %K          guidance constant of PP component of guidance law   (units: 1/s)
    %tau        time delay of PNP guidance law                      (units: stepSize)
%
%The output of this function in variable simPos is the simulated position
%of the bird. 
%
%Note: rows 1:tauMax of simPos will be identical to the corresponding rows 
%of birdPos, because the simulation only begins at time tauMax to ensure that 
%simulations assuming different time delays run on the same section of data.

%
%Copyright. Graham K. Taylor & Caroline H. Brighton, University of Oxford, November 2018.

function [simPos] = fitPNP(birdPos,birdVel,birdSpeed,lurePos,lureVel,stepSize,tauMax,NK,tau)

%Extracts guidance constants N and K from vector NK
N=NK(1); K=NK(2);

%Initializes matrices for bird's simulated position and velocity
simPos=birdPos*nan; simPos(1:tauMax,:)=birdPos(1:tauMax,:);
simVel=birdVel*nan; simVel(1:tauMax,:)=birdVel(1:tauMax,:);

for t=tauMax+1:length(birdPos)
   
    %% Uses PNP guidance law to compute bird's commanded acceleration at time step (t-1)
      
        %Calculates line-of-sight vector (r) lagged by tau
        r=lurePos(t-tau,:)-simPos(t-tau,:);
        
        %Calculates line-of-sight rate (lambdaDot) at time (t-delay)
        lambdaDot=cross(r, (lureVel(t-tau,:)-simVel(t-tau,:)))/sum(r.^2);
        
        %Defines velocity vector (v) lagged by tau
        v=simVel(t-tau,:);
    
        %Calculates deviation angle (delta) lagged by tau
        delta=acos(dot(r,v)/(norm(r)*norm(v)));
    
        %Creates unit vector normal to line of sight (r) and velocity (v)
%         n=cross(r,v)/(norm(r)*norm(v)); n=n/norm(n);
        n=cross(r,v)/norm(cross(r,v));%more efficient than above
    
        %Calculates commanded acceleration under delayed PNP guidance law
        birdAcc=N*cross(lambdaDot,simVel(t-1,:))-K*delta*cross(n,simVel(t-1,:));
        
        
    %% Computes bird's velocity and position at time step t
    
        %Applies commanded acceleration to bird's simulated velocity
        simVel(t,:)=simVel(t-1,:)+birdAcc/stepSize;
        
        %Adjusts simulated velocity of bird to match its measured speed
        simVel(t,:)=birdSpeed(t,:)*simVel(t,:)/norm(simVel(t,:));
        
        %Computes position of bird at time step t
        simPos(t,:)=simPos(t-1,:)+simVel(t,:)/stepSize;              
    
end