% Generate crosstrack error from current a/c position and waypoints.
% Input: xa - north position of a/c (ft)
%        ya - east position of a/c (ft)
%        xadot - north velocity of a/c (ft/sec)
%        yadot - east velocity of a/c (ft/sec)
% Parameters: xwp - north position of waypoints (ft)
%             ywp - east position of waypoints (ft)
%             firstwp - first waypoint (>1) (ft)
%             R=waypoint diameter (ft)
%             Kphi=Gain on Ccdot-Cdot
%             Kcdot=Gain on Cc-C
% Output: c - crosstrack error (ft) (positive when the aircraft is
%             to the left of the desired trrajectory)
%         cdot - cross track error rate (positive if the cross track error
%             is growing)
%         next - counter pointing to the next waypoint
%         PhiCom rad unlimited
function [sys,x0,str,ts] = cross_track3(t,x,u,flag,xwp,ywp,firstwp,R,Kphi,Kcdot)
switch flag,      
    case 0,     % Initialization
        [sys,x0,str,ts]=cross_track3InitializeSizes(firstwp);
    case 1,     % Compute derivatives of continuous states
        sys=cross_track3Derivatives(t,x,u);
    case 2,
        sys=cross_track3Update(t,x,u);
    case 3,     % Compute output vector
        sys=cross_track3Outputs(t,x,u,xwp,ywp,R,Kphi,Kcdot);  
    case 4,     % Compute time of next sample
        sys=cross_track3GetTimeOfNextVarHit(t,x,u);
    case 9,     % Finished 
        sys=cross_track3Terminate(t,x,u);
    otherwise   % Invalid input
        error(['Unhandled flag = ',num2str(flag)]);
end

%***********************************************************
%*                    cross_track3InitializeSizes                   *
%***********************************************************
function [sys,x0,str,ts]=cross_track3InitializeSizes(firstwp)
% Return the sizes of the system vectors, initial 
% conditions, and the sample times and offets.
sizes = simsizes;   % Create the sizes structure
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 4;
sizes.NumInputs      = 4;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);      % load sys with the sizes structure
next=firstwp; % index for first waypoint
set_param(gcb,'UserData',next); % store next to local storage
x0  = [];    % Specify initial conditions for all states 
str = [];    % str is always an empty matrix
ts  = [0 0]; %initialize the array of sample times


%***********************************************************
%*                    cross_track3Derivatives                       *
%***********************************************************
function sys=cross_track3Derivatives(t,x,u)
% Compute derivatives of continuous states
sys = [] ;

%***********************************************************
%*                    cross_track3Update                            *
%***********************************************************
function sys=cross_track3Update(t,x,u)
% Compute update for discrete states. If necessary, check for
% sample time hits.
sys = [];    % Empty since this model has no discrete states.

%***********************************************************
%*                    cross_track3Outputs                           *
%***********************************************************
function sys=cross_track3Outputs(t,x,u,xwp,ywp,R,Kphi,Kcdot)
xa=u(1); % north position of a/c (ft)
ya=u(2); % east position of a/c (ft)
xadot=u(3); % north velocity of a/c (ft/s)
yadot=u(4); % east velocity of a/c (ft/s)
% get counter next from local storage
next=get_param(gcb,'UserData'); 
% R=waypoint diameter (ft)

distance=sqrt((xwp(next)-xa)^2+(ywp(next)-ya)^2);
if distance <= R
    % a/c is considered to have reached waypoint once it enters
    % a circle of diameter R centered at the next waypoint
    newnext=next+1;
else
    % zone criterion for determining if a/c has reached a waypoint
    % refer to nextwaypoint.m for more details    
    newnext=nextwaypoint(next,xwp,ywp,xa,ya);
end
next=newnext; % update counter
% ERROR('MSGID', 'MESSAGE')
if(next>length(xwp))
    next
    str1=['Last waypoint has index ',num2str(length(xwp)),'.'];
    disp(str1)
    error('Error 1 in cross_track3. Aircraft needs a waypoint beyond the last one provided')
end
X1=xwp(next-1);
Y1=ywp(next-1);
X2=xwp(next);
Y2=ywp(next);
a=[Y2-Y1; X2-X1]; % vector from waypoint(next-1) to waypoint(next)
v=[ya-Y1; xa-X1]; % vector from waypoint(next-1) to aircraft
vdot=[yadot; xadot];
omega=atan2(a(1),a(2)); % angle to rotate vector a to vertical (rad)
% rotation matrix
rot_mat=[cos(-omega) sin(-omega); -sin(-omega) cos(-omega)];
v_rot=rot_mat*v; % rotate vector v 
vdot_rot=rot_mat*vdot; % rotate vector vdot 
% c is crosstrack error (ft)
% cdot is crosstrack error rate (ft/sec)
c=-v_rot(1); % after rotation, east component of vector v is c (ft)
cdot=-vdot_rot(1);% after rotation, east component of vector vdot is cdot (ft)
set_param(gcb,'UserData',next); % store next to local storage
PhiCom=0; % Insert control law here


% *** Start nominal control law
PhiCom=Kphi*(-Kcdot*c-cdot);
% *** End nominal control law

% Output vector
sys = [c cdot next PhiCom];

%***********************************************************
%*                    cross_track3GetTimeOfNextVarHit               *
%***********************************************************
function sys=cross_track3GetTimeOfNextVarHit(t,x,u)
% Return the time of the next hit for this block.  Note that 
% the result is absolute time.  Note that this function is 
% only used when you specify a variable discrete-time sample
% time [-2 0] in the sample time array in sampleTime = 1;    
sys = [] ;

%************************************************************
%*                    cross_track3Terminate                          *
%************************************************************
function sys=cross_track3Terminate(t,x,u)
% Perform any necessary tasks at the end of the simulation
sys = [];