function [OutputVector] = NetworkSendSimulink(InputVector);
%NetworkSendSimulink - called by Simulink to calculate vehicle trajectories.
%
%  USAGE - This function is called be the Simulink network example,
%     NetworkSendExample.mdl to generate vehicle trajectories to be sent
%     out as AVDS packets on the local network. Included in the output
%     vector is the calculated simulation time for each 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 $



%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ElapsedTime = InputVector(1);
VehicleID = InputVector(2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MaxNumberVehicles = 10;

TimeIncrement = 0.05;

iHorizontalSpacing = 50;
iVerticalSpacing = 10;
iInitAltitude = 10000;

NorthVelocity = 250.0;	%feet/sec	
EastVelocity = 0.0;	%feet/sec	
VerticalVelocity = 0.0;	%feet/sec

XAngularRate = 10.0; %degrees/sec
YAngularRate = 0.0; %degrees/sec
ZAngularRate = 0.0; %degrees/sec

DeltaElevator = 0;
DeltaAileron = 0;
DeltaRudder = 0;
EngineLevel = 200;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STORAGE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent SimulinkSimulationTime;
persistent 	NorthPosition;	% North position (feet)
persistent 	EastPosition;	% East positon (feet)
persistent 	UpPosition;	% Altitude (feet)
persistent 	XRotation;	%Roll Angle (deg)
persistent 	YRotation;	%Pitch Angle (deg)
persistent 	ZRotation;	%Heading Angle (deg)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ElapsedTime <= 0.0),
	SimulinkSimulationTime = zeros(MaxNumberVehicles,1);
	NorthPosition = zeros(MaxNumberVehicles,1);
	EastPosition = [iHorizontalSpacing:iHorizontalSpacing:MaxNumberVehicles*iHorizontalSpacing]';
	UpPosition = [-MaxNumberVehicles*iVerticalSpacing:iVerticalSpacing:-iVerticalSpacing]'+iInitAltitude;
	XRotation = zeros(MaxNumberVehicles,1);
	YRotation = zeros(MaxNumberVehicles,1);
   ZRotation = zeros(MaxNumberVehicles,1);
end;	%if(ElapsedTime <= 0.0),


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% build vehicle trajectories and store data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
SimulinkSimulationTime(VehicleID) = SimulinkSimulationTime(VehicleID) + TimeIncrement;

%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
% !!!! Prevent the Simulink simulation from getting behind:
% NOTE: this may not be desirable, but if Simulink is running too slow it is necessary
DeltaTime = TimeIncrement;
RealTimeMetric = ElapsedTime - SimulinkSimulationTime(VehicleID);
while(SimulinkSimulationTime < ElapsedTime), %NOTE: this can be <=
	SimulinkSimulationTime(VehicleID) = SimulinkSimulationTime(VehicleID) + TimeIncrement;
	DeltaTime = DeltaTime + TimeIncrement;
end;
%!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
          
NorthVelocityInc = NorthVelocity*TimeIncrement;
EastVelocityInc = EastVelocity*TimeIncrement;
VerticalVelocityInc = VerticalVelocity*TimeIncrement;

XAngularRateInc = XAngularRate*TimeIncrement;
YAngularRateInc = YAngularRate*TimeIncrement;
ZAngularRateInc = ZAngularRate*TimeIncrement;

NorthVelocityInc = NorthVelocity*DeltaTime;
EastVelocityInc = EastVelocity*DeltaTime;
VerticalVelocityInc = VerticalVelocity*DeltaTime;

XAngularRateInc = XAngularRate*DeltaTime;
YAngularRateInc = YAngularRate*DeltaTime;
ZAngularRateInc = ZAngularRate*DeltaTime;

NorthPosition(VehicleID) = NorthPosition(VehicleID) + NorthVelocityInc;
EastPosition(VehicleID) = EastPosition(VehicleID) + EastVelocityInc;
UpPosition(VehicleID) = UpPosition(VehicleID) + VerticalVelocityInc;
XRotation(VehicleID) = XRotation(VehicleID) + XAngularRateInc;
YRotation(VehicleID) = YRotation(VehicleID) + YAngularRateInc;
ZRotation(VehicleID) = ZRotation(VehicleID) + ZAngularRateInc;

OutputVector = [  NorthPosition(VehicleID),EastPosition(VehicleID),UpPosition(VehicleID), ...
      XRotation(VehicleID), YRotation(VehicleID), ZRotation(VehicleID), ...
      DeltaElevator, DeltaAileron , DeltaRudder, EngineLevel, ...
      SimulinkSimulationTime(VehicleID),RealTimeMetric];

return;	%NetworkSendSimulink
            
