function [sys,x0,str,ts] = guidance2(t,x,u,flag,EWP,NWP,HWP,VWP,ErrorPsiIC)
% function [sys,x0,str,ts] = guidance2(t,x,u,flag,EWP,NWP,HWP,VWP)

switch flag,      
  case 0,         % Initialization
    [sys,x0,str,ts]=guidance2InitializeSizes(t,x,u,flag,EWP,NWP,HWP,VWP,ErrorPsiIC);
  case 1,         % Compute derivatives of continuous states
    sys=guidance2Derivatives(t,x,u,flag) ;
  case 2,
    sys=guidance2Update(t,x,u,flag);
  case 3,
    sys=guidance2Outputs(t,x,u,flag,EWP,NWP,HWP,VWP);  % Compute output vector
  case 4,                   % Compute time of next sample
    sys=guidance2GetTimeOfNextVarHit(t,x,u,flag);
  case 9,                   % Finished. Do any needed 
    sys=guidance2Terminate(t,x,u,flag);
  otherwise                 % Invalid input
    error(['Unhandled flag = ',num2str(flag)]);
end
 
%***********************************************************
%*                    guidance2InitializeSizes                   *
%***********************************************************
function [sys,x0,str,ts]=guidance2InitializeSizes(t,x,u,flag,EWP,NWP,HWP,VWP,ErrorPsiIC)
% 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;   % u is used in Outputs
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes); 
% load sys with the sizes structure
next=2; njump=0; 
DataMat=[next ErrorPsiIC njump];
set_param(gcb,'UserData',DataMat); % use 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


%***********************************************************
%*                    guidance2Derivatives                       *
%***********************************************************
function sys=guidance2Derivatives(t,x,u,flag)
% Compute derivatives of continuous states
% Get elements of the state vector
sys = [];   % rad/sec

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

%***********************************************************
%*                    guidance2Outputs                           *
%***********************************************************
function sys=guidance2Outputs(t,x,u,flag,EWP,NWP,HWP,VWP)
NA=u(1);     % North position of the aircraft, feet
EA=u(2);     % East position of the aircraft, feet
Vnorth=u(3);    % Northerly velocity in NED coordinate, ft/s
Veast=u(4);     % Easterly velocity in NED coordinate, ft/s

DataMat=get_param(gcb,'UserData'); % get next from local storage
%DataMat=[next ErrorPsi njump];
next=DataMat(1); ErrorPsiOLD=DataMat(2); njump=DataMat(3);

[newnext]=nextwaypoint(next,EWP,NWP,EA,NA);
%function [newnext]=nextwaypoint(next,EWP,NWP,EA,NA)
next=newnext;

if next>length(EWP)
    disp('You must add more waypoints for next+1 to be defined at this location')
    disp('See function guidance2Outputs')
end


%tanPsiC=(EWP(next)-EA)/(NWP(next)-NA);
de=EWP(next)-EA;
dn=NWP(next)-NA;
num=de*Vnorth-dn*Veast;
den=dn*Vnorth+de*Veast;
ErrorPsi=atan2(num,den);

%PsiC=atan2((EWP(next)-EA),(NWP(next)-NA));
%PsiGT=atan2(Veast,Vnorth);
%ErrorPsi2=PsiC-PsiGT

% Remove jumps of 2*pi using rmjmp
%[Zout,njump]=rmjmp(Zk,Zkm1,njump,jmp,tol)
[ErrorPsiOUT,njump]=rmjmp(ErrorPsi,ErrorPsiOLD,njump,2*pi,pi);

% Store results in local memory for this s-function
DataMat=[next ErrorPsi njump];
set_param(gcb,'UserData',DataMat); % save to local storage

sys = [ErrorPsiOUT,VWP(next-1),HWP(next-1),next];


%***********************************************************
%*                    guidance2GetTimeOfNextVarHit                *
%***********************************************************
function sys=guidance2GetTimeOfNextVarHit(t,x,u,flag)
% 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 = [] ;

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

