function AVDS_AC_Matlab()
%  AVDS_AC_Matlab.m - This is a very simple example of the AVDS to MATLAB shared memory connection.
%  The flight control system (FCS) model is executed inside of AVDS. The aircraft (AC) 
%  model is implemented in MATLAB. Inputs to the AC are sent through shared memory from 
%  AVDS to MATLAB, Outputs from the AC are sent from MATLAB through shared memory to AVDS.
%
%  To start the simulation:
%  1) Start AVDS
%  2) Load the simulation initialization file "MATLAB_AC.sim.ini". From the AVDS 
%     menu select "File->Simulation Init->Open" and choose the file "MATLAB_AC.sim.ini".
%  3) In MATLAB run the AVDS_AC_Matlab.m script
%  4) In AVDS, choose Simulation Mode and press the "Start" button.
%
%  To stop the simulation:
%  1) Press the AVDS "Stop" button.
%
%
%	see also: SimulationConnection()


%   Steve Rasmussen, October 2000
%   Copyright (c) 2000 by RasSimTech Ltd
%   $Revision: 1.0 $  $Date: 2000/10/31 00:00:00 $
%   $Revision: 1.1 $  $Date: 2000/11/26 00:00:00 $
%   $Revision: 1.2 $  $Date: 2000/12/05 00:00:00 $
%   $Revision: 1.3 $  $Date: 2001/04/09 00:00:00 $

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

% this is the connection ID number for the MATLAB_AC aircraft in AVDS
ConnectionID = 2;

% 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:
InputVector = zeros(16,1);	%intitialize input vector - the contents of this vector will be received from AVDS


% 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, 
%   and can be used to send other signals to AVDS:
OutToAVDS01 = 16; OutToAVDS02 = 17; OutToAVDS03 = 18; OutToAVDS04 = 19; OutToAVDS04 = 20;
OutToAVDS06 = 21; OutToAVDS07 = 22; OutToAVDS08 = 23; OutToAVDS09 = 24; OutToAVDS10 = 25;
% 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;

MatlabSimulationTime = 0.0;	% local function's simulation time
WasRunning = 0;	% flag to prevent exit before simulation has ran once.
                      
%Open the AVDS shared memory link
SimulationConnection('OpenConnection',ConnectionID,'AC');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       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;

if(WasRunning == 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)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%       Simulation Main Loop      %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while 1	% loop until the break is encountered
   %Retrive input vector from AVDS (from shared memory link)
   InputVector = SimulationConnection('ReceiveData',ConnectionID);
   %The signal InputVector(InbRunning) is a flag that tests whether AVDS is running in Simulation mode (1) or not (0).
   % The "WasRunning" flag is used to know when to stop the simulation
   if ((InputVector(1) == 0) & (WasRunning == 1)),
      break;	%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;
      
      %Retrive the current simulation time from AVDS.
      AVDSSimulationTime = InputVector(InElapsedTime);
      if (MatlabSimulationTime < AVDSSimulationTime), %NOTE: this can be <= 
         % increment Matlab simulation time
         MatlabSimulationTime = MatlabSimulationTime + TimeIncrement;
         
         %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
         % !!!! 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;
         while(MatlabSimulationTime < AVDSSimulationTime), %NOTE: this can be <=
            MatlabSimulationTime = MatlabSimulationTime + TimeIncrement;
            DeltaTime = DeltaTime + TimeIncrement;
         end;
          %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
        
      	% update the simulation
      	ULong = [InputVector(IndElevator)];
      	TrimElevator = 0.1;
      	LongCmd = (-ULong*pi/30 + XLong(Index_q)) + TrimElevator;
      	ULat = [InputVector(IndAileron);InputVector(IndRudder)];
      	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));
         
         % 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(OutToAVDS01) = -InputVector(IndElevator); %radians
   		OutputVector(OutToAVDS02) = InputVector(IndAileron); %radians
			%send output vector to AVDS
      	SimulationConnection('SendData',ConnectionID,OutputVector);
		end;	%if (MatlabSimulationTime > AVDSSimulationTime),
   end;	%if ((InputVector(1) == 0( & (WasRunning == 1))
end;
%Close the connection with AVDS
SimulationConnection('CloseConnection');

return; %AVDS_AC_Matlab
