function [sys,x0,str,ts] = GuideEE392m(t,x,u,flag)
% GuideEE392m - UAV guidance law for EE392m, Stanford, Spring 2005
% Based on Simulink SFUNDSC1 example M-file S-function with inherited 
% sample time - see Matlab Help for SFUNDSC1 documentation.  
%   

switch flag,

  %%%%%%%%%%%%%%%%%%
  % Initialization %
  %%%%%%%%%%%%%%%%%%
  case 0,
   [sys,x0,str,ts] = GuideInitializeSizes;
  
  %%%%%%%%%%  
  % Update %
  %%%%%%%%%%
  case 2,                                               
    sys = GuideUpdate(t,x,u);
    
  %%%%%%%%%%
  % Output %
  %%%%%%%%%%
  case 3,                                               
    sys = GuideOutputs(t,x,u);    

  %%%%%%%%%%%%%
  % Terminate %
  %%%%%%%%%%%%%
  case 9,                                               
    sys = [];

  otherwise
    error(['unhandled flag = ',num2str(flag)]);
end
%end GuideEE392m

%
%=============================================================================
% GuideInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%=============================================================================
%
function [SysSizes,x0,str,ts] = GuideInitializeSizes

sizes = simsizes;

sizes.NumContStates  = 0;
sizes.NumDiscStates  = 1;
sizes.NumOutputs     = 1;
sizes.NumInputs      = 7;
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 1;

SysSizes = simsizes(sizes);

x0  = 0;
str = [];
ts  = -1;       % Inherited sampling time 

% end GuideInitializeSizes

%
%=======================================================================
% GuideUpdate - discrete state update at sample times 
%   THIS IS THE MAIN GUIDANCE LAW FUNCTION  
%=======================================================================
%
function Rcmd = GuideUpdate(t,x,u)
% INPUTS: 
%   t - simulation time (sec) 
%   x - update state (previous output) 
%   u - input vector 
%       % UAV states
%       u(1) - Velocity (m) 
%       u(2) - Heading angle (deg) 
%       u(3) - Latitude, N-coordinate (m) 
%       u(4) - Longitude, E-coordinate (m) 
%       % Guidance target
%       u(5) - Target heading (deg) 
%       u(6) - Waypoint latitude, N-coordinate (m)
%       u(7) - Waypoint longitude, E-coordinate (m)
% OUTPUT: 
%    Rcmd - yaw rate command for the UAV (rad/sec) 

%
% Parameters of the guidance law - should be the same as set in the Simulink
% 
%% Sampling time: 
%   See the ZOH block at the input of this Matlab S-function 
DT = 2;   % sampling time in sec; Sample period of 2 seconds (0.5Hz)

%% Controller states 
persistent Yp uold uoldold Ydis
if   size(Yp) ~= [1 1],   Yp = 0; end;       %   Yp - past step prediction
if size(uold) ~= [1 1], uold = 0; end;       % uold - past step control 
if size(uoldold)~=[1 1], uoldold = 0; end; % uoldold - 2 step old control 
if size(Ydis) ~= [1 1], Ydis = 0; end;       % dist - disturbance estimate


%%
%% Input Data 
Velocity     = u(1);           % m
HeadingAngle = u(2)*pi/180;    % rad 
Ncoordinate = u(3);            % m 
Ecoordinate = u(4);            % m 
HeadingTarget= u(5)*pi/180;    % rad 
Nwaypoint   = u(6);            % m 
Ewaypoint   = u(7);            % m 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%% Guidance law
RT = [cos(HeadingTarget) sin(HeadingTarget); 
     -sin(HeadingTarget) cos(HeadingTarget)];    % rotation matrix     
dx = Ncoordinate - Nwaypoint;       % X offset 
dy = Ecoordinate - Ewaypoint;       % Y offset
XY = RT*[dx; dy];   % Coordinates relative to the target track  
X  = XY(1);         % target track coordinate
Y  = XY(2);         % distance to the target track
V  = Velocity*[cos(HeadingAngle); sin(HeadingAngle)];  % vehicle velocity 
%V  = [VNorth;  VEast];  % vehicle velocity 
%Velocity = sqrt(VNorth^2 + VEast^2); 
VXY= RT*V;            % Target track velocity
VX = VXY(1);         % velocity along target track  
VY = VXY(2);         % velocity across target track  

%% Guidance law from the AIAA2001-016.pdf  paper by Marius Niculescu
% 
%  This guidance law should be replaced by MPC (RHC) law
%
KR = 3.0e-6;
k  = 0.2; 
ET = k*X*VY - Y*VX;
Rcmd = KR*ET; 

%end GuideUpdate

%
%=======================================================================
% GuideOutputs
% The output vector for the S-function 
%=======================================================================
%
function Rcmd = GuideOutputs(t,x,u)
Rcmd = x;

%end GuiseOutputs