function ydot = dynamics(t, Y, Options, PARAMETERS)

%	These are the equations of motion.  The vehicle is treated as a point with
%	an initial velocity vector to follow (ox, oy, oz)
%	This function is in the SECOND ORDER stage of development.

%	VARIABLES and UNITS
%
%	conventions:	Large arrays are in all caps
%						Vectors and other variables are in mixed case, as appropriate
%
%	ydot				:the result vector of this function. 
%	Isp				:Specific Impulse, in s.
%	g					:Gravity, in m/s2.
%	x, y, z			:position in the x, y, or z direction (Earth Frame), in m.
%	vx, vy, vz		:velocity in the x,y,or z direction (Body Frame), in m/s.
%	ox, oy, oz		:orientation (Earth Frame), in deg.
%	ovx, ovy, ovz	:orientation rates (Body Frame), in deg/s.
%	mf					:mass of the fuel, in kg.
%	inert_mass		:mass of the rocket, less the fuel, in kg.
%	Ft					:Thrust force vector, in N.
%	Fd					:Drag force vector, in N.
%	Fw					:Force due to winds aloft, in N.
%	Fl					:Lift force vector, in N.


% GLOBALS
global data;
global index;


% don't record data if this is a repeat itteration
if index > 1
   if t == data(index-1, 1)
      index = index - 1;
   end
end


% create another column to put data into
if index > 1
	data(index,:) = zeros(1,length(data(1,:)));
end

%	CONSTANTS
r2d = 57.296;							% conversion for radians to degrees
inert_mass = data(index,20);		% kg



%	BREAKOUT

x = Y(1);							% position
y = Y(2);
z = Y(3);
vx = Y(4);							% velocity
vy = Y(5);
vz = Y(6);
ox = Y(7);							% axis orientation
oy = Y(8);
oz = Y(9);
ovx = Y(10);						% axis rate
ovy = Y(11);
ovz = Y(12);



%	MAIN PROGRAM
time = t;
if index > 1
   dt = t - data((index-1),1);
else
   dt = 0;
end


g = gravity(z);

%ft = enginethrust(t,z); %(Pc, Pox, z);
Ft = [10000 0 800];

Fd = aerodrag(vx,vy,vz,z);

%Fl = .05 * .5 * density(z) * (vx^2 + vy^2) * .15;

%mp = ecMFUEL(index) + eoMOX(index);
mp = 75;
mt = inert_mass + mp;



%	RESULTANT EQUATIONS OF MOTION

% Boost phase (while there's still fuel, and it's in the air)
if (mp > 0 & z > 0)
   if (z > 0)
   	ydot(1) = vx;									% x-Velocity result
   	ydot(2) = vy;									% y-Velocity result
      ydot(3) = vz;									% z-Velocity result
      ydot(4) = Ft(1)/mt + Fd(1)/mt;				% x-Acceleration result
   	ydot(5) = Ft(2)/mt + Fd(2)/mt;				% y-Acceleration result
      ydot(6) = Ft(3)/mt + Fd(3)/mt - g;% + Fl/mt;			% z-Acceleration result
      
   	ydot(7) = ovx;									% omega Roll
   	ydot(8) = ovy;									% omega Pitch
      ydot(9) = ovz;									% omega Yaw
      ydot(10)= ovx*dt;								% alpha Roll
      ydot(11)= ovy*dt;								% alpha Pitch
      ydot(12)= ovz*dt;								% alpha Yaw
   end
   
   
% Coast phase
else
   
   % Either in the air still
   if (z > 0)
       
      ydot(1) = vx;							% x-Velocity result
  		ydot(2) = vy;							% y-Velocity result
      ydot(3) = vz;							% z-Velocity result
      ydot(4) = Fd(1)/mt;					% x-Acceleration result
      ydot(5) = Fd(2)/mt;					% y-Acceleration result
      ydot(6) = Fd(3)/mt - g;%Fl/mt;	% z-Acceleration result
      
		ydot(7) = -ox*dt;						% omega Roll
  	 	ydot(8) = -oy*dt;						% omega Pitch
      ydot(9) = -oz*dt;						% omega Yaw   
      ydot(10)= -1;								% alpha Roll
      ydot(11)= -1;								% alpha Pitch
      ydot(12)= -1;								% alpha Yaw
      
   % Or impacted into the ground
   else
      ydot(1) = vx/100;						% Smashing into the ground.
      ydot(2) = vy/100;							
      ydot(3) = vz/100;      				
      ydot(4) = -vx;				
  		ydot(5) = -vy;					
      ydot(6) = -vz;
      
      ydot(7)  = 0;						
   	ydot(8)  = 0;							
      ydot(9)  = 0;
      ydot(10) = 0;						
   	ydot(11) = 0;							
      ydot(12) = 0;								
   end
   
end




% update data array

data(index,1) =  time;
data(index,2) =  dt;
data(index,5) =  index;
data(index,10) = x;
data(index,11) = y;
data(index,12) = z;
data(index,13) = ydot(1);
data(index,14) = ydot(2);
data(index,15) = ydot(3);
data(index,16) = ydot(4);
data(index,17) = ydot(5);
data(index,18) = ydot(6);
data(index,19) = ox * r2d;
data(index,20) = oy * r2d;
data(index,21) = oz * r2d;
data(index,22) = ydot(7);
data(index,23) = ydot(8);
data(index,24) = ydot(9);
data(index,25) = ydot(10);
data(index,26) = ydot(11);
data(index,27) = ydot(12);
data(index,16) = inert_mass;




index = index + 1;

ydot = ydot';