function [OutputVector] = AVDS_FCS_Simulink(InputVector)
%AVDS_FCS_Simulink - called by Simulink to simulate a flight control system (FCS).
%
%  USAGE - This function is called be the Simulink FCS interactive simulation 
%     example, ExampleFCS.mdl to simulatate the a FCS that is connected to the vehicle
%     model in 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;

%make sure these variables get initialized
if (isempty(WasRunning)), WasRunning= 0; end;
if (isempty(SimulinkSimulationTime)), SimulinkSimulationTime = 0; end;


% Define the indices for the input vector:
InbRunning = 1; 
InStickPitch = 2;
InStickRoll = 3;
InStickYaw = 4;
InStickThrottle = 5;
InP = 6;
InQ = 7;
InR = 8;
InElapsedTime = 9;
% The following indicies in the input vector have not been used by this Aircraft, 
%   and can be used to receive other signals from AVDS:
ToMatlab01 = 10; ToMatlab02 = 11; ToMatlab03 = 12; ToMatlab04 = 13; ToMatlab05 = 14;
ToMatlab06 = 15; ToMatlab07 = 16; ToMatlab08 = 17; ToMatlab09 = 18; ToMatlab10 = 19;
% Total size of input (from AVDS) vector, must be the same size as the defintion found in the 
%   SimulationConnection function for the FCS type:


% Define the indices for the output vector:
OutElevatorCmd = 1;
OutAileronCmd = 2;
IndexRudderCmd = 3;
IndexThrottleCmd = 4;
% The following indicies in the output vector have not been used by this FCS, 
%   except as noted in the next section, and can be used to send other signals to AVDS:
ToAVDS01 = 5; ToAVDS02 = 6; ToAVDS03 = 7; ToAVDS04 = 8; ToAVDS05 = 9;
ToAVDS06 = 10; ToAVDS07 = 11; ToAVDS08 = 12; ToAVDS09 = 13; ToAVDS10 = 14;

% used OutToAVDS09 to output a timing metric for deisplay in AVDS Charts/Simulink diplay blocks
OutRealTimeMetric = ToAVDS09;
% used OutToAVDS10 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.
OutputVector = zeros(14,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;
end;	%if (WasRunning == 0),


PitchGain = 1.0;
RollGain = 0.8;
YawGain = 1.0;


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

RealTimeMetric = 0;

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

%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;
          %!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
          
      OutputVector(OutElevatorCmd) = PitchGain * (InputVector(InStickPitch) + InputVector(InQ));
   	OutputVector(OutAileronCmd) = RollGain * (InputVector(InStickRoll) + InputVector(InP));
   	OutputVector(IndexRudderCmd) = YawGain * (InputVector(InStickYaw) - InputVector(InR));
   	OutputVector(IndexThrottleCmd) = 0.45;
   	OutputVector(OutRealTimeMetric) = RealTimeMetric;
   	OutputVector(OutSimulinkSimulationTime) = SimulinkSimulationTime;
   end;	%if ((InputVector(1) == 0) & (WasRunning == 1)),
   
return;	%AVDS_FCS_Simulink