function [OutputVector] = AVDS_AC_Simulink(InputVector)
%AVDS_AC_Simulink - called by Simulink to simulate vehicle dynamics.
%
%  USAGE - This function is called be the Simulink AC interactive simulation 
%     example, ExampleAC.mdl to simulation vehicle dynamics to be sent
%     out to AVDS through the shared memory connection. Included in the output
%     vector is the calculated simulation time for the vehicle. This time
%     is used by Simulink to syncronize the simulation to real-time.
%

%   Steve Rasmussen, April 2001
%   Copyright (c) 2001 by RasSimTech Ltd
%   $Revision: 1.0.0 $  $Date: 2001/04/15 00:00:00 $


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       Setup Simulation Parameters      %%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent WasRunning;
persistent SimulinkSimulationTime;
persistent 	PositionX;	% initial East position
persistent 	PositionY;	% initial North positon
persistent 	PositionZ;	% initial altitude
persistent 	RotationX;	%Roll Angle
persistent 	RotationY;	%Pitch
persistent 	RotationZ;	%initial heading

%make sure these variables get initialized
if (isempty(WasRunning)), WasRunning= 0; end;
if (isempty(SimulinkSimulationTime)), SimulinkSimulationTime = 0; end;
if (isempty(PositionX)), PositionX = 0; end;
if (isempty(PositionY)), PositionY = 0; end;
if (isempty(PositionZ)), PositionZ = 0; end;
if (isempty(RotationX)), RotationX = 0; end;
if (isempty(RotationY)), RotationY = 0; end;
if (isempty(RotationZ)), RotationZ = 0; end;


% Define the indices for the input vector:
InbRunning = 1; 
IndElevator = 2; 	IndAileron = 3; 	IndRudder = 4;	IndThrottle = 5; 
InElapsedTime = 6;
% The following indicies in the input vector have not been used by this Aircraft, 
%   and can be used to receive other signals from AVDS:
InToMATLAB01 = 7;	InToMATLAB02 = 8;	InToMATLAB03 = 9;	InToMATLAB04 = 10;	InToMATLAB05 = 11;	
InToMATLAB06 = 12;	InToMATLAB07 = 13;	InToMATLAB08 = 14;	InToMATLAB09 = 15;	InToMATLAB10 = 16;	
% Total size of input (from AVDS) vector, must be the same size as the defintion found in the 
%   SimulationConnection function for the AC type:


% Define the indices for the output vector:
OutPositionX = 1;	OutPositionY = 2;	OutPositionZ = 3;
OutRotationX = 4;	OutRotationY = 5;	OutRotationZ = 6;
OutAlpha  = 7;	OutBeta  = 8;
OutG  = 9;
OutVelocity  = 10;
OutM  = 11;
OutDeltaRudder  = 12;	OutDeltaElevator  = 13;	OutDeltaAileron  = 14;
OutEngine = 15;
% The following indicies in the output vector have not been used by this Aircraft, 
%   except as noted in the next section, and can be used to send other signals to AVDS:
ToAVDS01 = 16; ToAVDS02 = 17; ToAVDS03 = 18; ToAVDS04 = 19; ToAVDS04 = 20;
ToAVDS06 = 21; ToAVDS07 = 22; ToAVDS08 = 23; ToAVDS09 = 24; ToAVDS10 = 25;

% used ToAVDS01 to output a second elevator deflection value to AVDS (used by the F/A-18a craft file)
OutDeltaElevatorStarboard = ToAVDS01;
% used ToAVDS02 to output a second aileron deflection value to AVDS
OutDeltaAileronStarboard = ToAVDS02;
% used ToAVDS09 to output a timing metric for deisplay in AVDS Charts/Simulink diplay blocks
OutRealTimeMetric = ToAVDS09;
% used ToAVDS10 to output the Simulink simulation time to syncronized Simulink to AVDS time and
% for display in AVDS charts (if desired).
OutSimulinkSimulationTime = ToAVDS10;

% Total size of output (to AVDS) vector, must be the same size as the defintion found 
%   in the SimulationConnection function for the AC type:
OutputVector = zeros(25,1); 	%intitialize output vector - the contents of this vector will be sent to AVDS

% Delta time for model updates. 
% -  Using a simple rectangular integration
TimeIncrement = 0.05;

if(InputVector(InbRunning) == 0),
	WasRunning = 0;	% flag to prevent exit before simulation has ran once.
   return;
end;

if (WasRunning == 0),
   SimulinkSimulationTime = 0.0;
	PositionX  = 0;	% initial East position
	PositionY  = 0;	% initial North positon
	PositionZ  = 20000;	% initial altitude
	RotationX = 0;	%Roll Angle
	RotationY = 0;	%Pitch
	RotationZ = 0;	%initial heading
end;	%if (WasRunning == 0),

                      

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       Initialize Dynamics      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ALong = [ ...
   -0.0059    0.0237         0  -32.2000
   -0.1085   -0.5161  651.3479         0
    0.0000   -0.0019   -0.6439         0
    0         0    1.0000         0];
BLong = [ ...
         0
  -25.1185
   -1.6895
         0];
V = 673;	%initial total velocity (assume initially u = V)
XLong = [  ...
      V
   	0
   	0
      0];
   
Index_u =1;
Index_w =2;
Index_q =3;
Index_theta =4;

 
   
ALat = [ ...
   -0.1067         0 -673.0000   32.2000		
   -0.0041   -0.8428    0.3281         0		
    0.0015   -0.0167   -0.2565         0		
         0    1.0000         0         0		
      ];
BLat = [ ...
		0    9.5858
    0.2212    0.1501
    0.0094   -0.6234
         0         0
      ];
XLat = [ ...
      0
   	0
   	0
   	0];
   
Index_v =1;
Index_p =2;
Index_r =3;
Index_phi =4;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       Simulation Main Loop      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Retrive the current simulation time from AVDS.
AVDSSimulationTime = InputVector(InElapsedTime);
% increment Matlab simulation time
SimulinkSimulationTime = SimulinkSimulationTime + TimeIncrement;

RealTimeMetric = 0;

%The variable InputVector(1) is a flag that test whether AVDS is running in Simulation mode (1) or not (0).
   if ((InputVector(1) == 0) & (WasRunning == 1)),
     %If the simulation has run and the alive flag is set to zero, stop the simulation. (Alive flag is set to 1 when AVDS is operating in Simulation mode)
   elseif (InputVector(1) == 1),	% if AVDS is running in Simulation mode 
      WasRunning = 1;
      
         
         %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
         % !!!! Prevent the Matlab simulation from getting behind:
         % NOTE: this may not be desirable, but if Matlab is running too slow it is necessary
         DeltaTime = TimeIncrement;
         RealTimeMetric = AVDSSimulationTime - SimulinkSimulationTime;
         while(SimulinkSimulationTime < AVDSSimulationTime), %NOTE: this can be <=
            SimulinkSimulationTime = SimulinkSimulationTime + TimeIncrement;
            DeltaTime = DeltaTime + TimeIncrement;
         end;
          %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        
      	% update the simulation
      	ULong = [InputVector(IndElevator)]*10.0;
      	TrimElevator = 0.1;
      	LongCmd = (-ULong + XLong(Index_q)) + TrimElevator;
      	ULat = [InputVector(IndAileron)*100.0;InputVector(IndRudder)*100.0];
      	LatCmd = [ULat(1)-XLat(Index_p);(ULat(2)+XLat(Index_r))];
         
         XLong = XLong + (ALong*XLong + BLong*LongCmd)*TimeIncrement;
         XLat = XLat + (ALat*XLat + BLat*LatCmd)*TimeIncrement;
      	V = sqrt(XLong(Index_u)*XLong(Index_u) + XLong(Index_w)*XLong(Index_w) + XLat(Index_v)*XLat(Index_v));
         RotationX = RotationX + DeltaTime * XLat(Index_p);
         RotationY = RotationY + DeltaTime * XLong(Index_q);
         RotationZ = RotationZ - DeltaTime * XLat(Index_r);
         PositionX = PositionX + DeltaTime * V*cos(RotationZ);
         PositionY = PositionY + DeltaTime * V*sin(RotationZ);
         PositionZ = PositionZ - DeltaTime * V*sin(XLong(Index_theta));
   end;	%if ((InputVector(1) == 0( & (WasRunning == 1))
         
        	% set up output vector to be sent to AVDS        
   		OutputVector(OutPositionX) = PositionX;
   		OutputVector(OutPositionY) = PositionY;
   		OutputVector(OutPositionZ) = PositionZ;
      	OutputVector(OutRotationX) = -RotationX * pi/180;
      	OutputVector(OutRotationY) = -RotationY * pi/180;
      	OutputVector(OutRotationZ) = -RotationZ * pi/180;
      	OutputVector(OutAlpha) = -asin(XLong(Index_w)/V);
      	OutputVector(OutBeta) = asin(XLat(Index_v)/V);
      	OutputVector(OutG) = 0.0;
   		OutputVector(OutVelocity) = V;
   		OutputVector(OutM) = 0.0;
   		OutputVector(OutDeltaRudder) = InputVector(IndRudder); %radians
   		OutputVector(OutDeltaAileron) = -InputVector(IndAileron); %radians
   		OutputVector(OutDeltaElevator) = -InputVector(IndElevator); %radians
         OutputVector(OutEngine) = 0; %0-255
         % the next two outputs are used to delflect the respective starboard control effectors
   		OutputVector(OutDeltaElevatorStarboard) = InputVector(IndElevator); %radians
         OutputVector(OutDeltaAileronStarboard) = InputVector(IndAileron); %radians
         
   		OutputVector(OutRealTimeMetric) = RealTimeMetric; %
    		OutputVector(OutSimulinkSimulationTime) = SimulinkSimulationTime; %
  
   return; %AVDS_AC_Simulink_Simulink
