function ydot = kinematics(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.
%	ax, ay, az		:acceleration in the x,y,or z direction (Body Frame), in m/s2.
%	ox, oy, oz		:orientation (Earth Frame), in deg.
%	p, q, r			:rotation rates (Body Frame), in deg/s.
%	pdot, qdot, rdot: body rotation rate derivatives, in deg/s2.
%	e0, e1, e2, e3	:quaternians.
%	e0dot, e1dot,	:quaternian derivatives.
%	e2dot, e3dot	
%	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;
global e;


% 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 in 'data' to put data into
if index > 1
	data(index,:) = zeros(1,length(data(1,:)));
end



%	CONSTANTS
r2d = 57.296;							% conversion for radians to degrees


if index > 1
	inert_mass = data(index-1,45);		% kg
else
   inert_mass = data(index,45);
end


%	BREAKOUT (in Earth Frame)

x = Y(1);		% Earth Frame position
y = Y(2);
z = Y(3);
u = Y(4);		% Body Frame velocity
v = Y(5);
w = Y(6);
e0 = Y(7);		% Earth Frame orientation (quaternians)
e1 = Y(8);
e2 = Y(9);
e3 = Y(10);
p  = Y(11);		% Body Frame angular rotation rates
q  = Y(12);
r  = Y(13);



% convert velocity from Body Frame to Earth Frame
%vb = [ (e0^2+e1^2-e2^2-e3^2) 2*(e0*e3+e1*e2)       2*(e1*e3-e0*e2); ...5
%							2*(e1*e2-e0*e3)       (e0^2-e1^2+e2^2-e3^2) 2*(e0*e51+e2*e3); ...
%                    2*(e0*e2+e1*e3)       2*(e2*e3-e0*e1)        (e0^2-e1^2-e2^2+e3^2)] ...
%                 * [vxe; vye; vze];
%              
%u = vb(1);
%v = vb(2);
%w = vb(3);

Ve = [ (e1^2+e0^2-e2^2-e3^2) 2*(e1*e2+e3*e0)       2*(e1*e3-e2*e0); ...
							2*(e1*e2-e3*e0)       (e2^2+e0^2-e1^2-e3^2) 2*(e2*e3+e1*e0); ...
                     2*(e1*e3+e2*e0)       2*(e2*e3-e1*e0)        (e3^2+e0^2-e1^2-e2^2)] ...
                  	* [u; v; w];
vxe = Ve(1);
vye = Ve(2);
vze = Ve(3);


% convert attitude rates from Body Frame to Earth Frame
%pqrb = [ (e0^2+e1^2-e2^2-e3^2) 2*(e0*e3+e1*e2)       2*(e1*e3-e0*e2); ...
%							2*(e1*e2-e0*e3)       (e0^2-e1^2+e2^2-e3^2) 2*(e0*e1+e2*e3); ...
%                     2*(e0*e2+e1*e3)       2*(e2*e3-e0*e1)        (e0^2-e1^2-e2^2+e3^2)] ...
%                     * [pe; qe; re];
%   						
%p = pqrb(1);
%q = pqrb(2);
%r = pqrb(3);

Oe = [ (e1^2+e0^2-e2^2-e3^2) 2*(e1*e2+e3*e0)       2*(e1*e3-e2*e0); ...
							2*(e1*e2-e3*e0)       (e2^2+e0^2-e1^2-e3^2) 2*(e2*e3+e1*e0); ...
                     2*(e1*e3+e2*e0)       2*(e2*e3-e1*e0)        (e3^2+e0^2-e1^2-e2^2)] ...
                  	* [p; q; r];
pe = Oe(1);
qe = Oe(2);
re = Oe(3);


% create euler angles from quaternians
phi   = r2d * atan2(2*(e0*e1 + e2*e3), (e0^2 + e3^2 - e1^2 - e2^2));
theta = r2d * asin(2*(e0*e2 - e1*e3));
psi   = r2d * atan2(2*(e0*e3 + e1*e2), (e0^2 + e1^2 - e2^2 - e3^2));


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


%g = gravity(z);
%aoa= : wind vector, v, o;
%m  = mass(t);
%cg = centerofgravity(m);
%[Ft;Mt] = thrust(t);
%[Fl Ml; Fd Md] = aero(v,o,z,aoa);


% moments of inertia
Ix = 20;
Iy = 15;
Iz = 15;
Ixy = 0;
Ixz = 0;
Iyx = 0;
Iyz = 0;
Izx = 0;
Izy = 0;
I=[Ix Ixy Ixz; ...
 	 Iyx Iy Iyz; ...
 	 Izx Izy Iz];


thrust = -800 * t + 4000;
if thrust > 0
	Ft = [thrust 0 0];
else 
   Ft = [0 0 0];
end

g = gravity(z);	% Earth Frame
drag = aerodrag(u, v, w, z);	% Body Frame
Fdx = drag(1);
Fdy = drag(2);
Fdz = drag(3);
Mdx = drag(4);
Mdy = drag(5);
Mdz = drag(6);

% fake temp moments
if index > 2
alphaz = atan(w/u);
%alphay = tan(v/u);
Mtempx = 0;
Mtempy = -alphaz * sqrt(u^2+v^2+w^2) * 5;
Mtempz = 0;
else
   Mtempx = 0;
   Mtempy = 0;
   Mtempz = 0;
end

%mp = (15-time)*4;
mt = inert_mass; % + mp;

% Body forces
Fx = Ft(1) + Fdx;
Fy = Ft(2) + Fdy;
Fz = Ft(3) + Fdz;

% Body moments
Mx = Mdx + Mtempx;
My = Mdy + Mtempy;
Mz = Mdz + Mtempz;




%	RESULTANT EQUATIONS OF MOTION

% Body Frame accelerations
udot = (Fx)/mt + 2*g*(e0*e2+e1*e3) - q*w + r*v;
vdot = (Fy)/mt + 2*g*(e2*e3-e0*e1) - r*u + p*w;
wdot = (Fz)/mt + g*(e0^2-e1^2-e2^2+e3^2) - p*v + q*u;

vxedot = 0;
vyedot = 0;
vzedot = 0;


% Full Body Frame rotational accelerations
% From: Msum = Hdot + cross(w,H)
%pdot = (Mx + Ixy*qdot + Ixz*rodt - q*(-Izx*p - Izy*q + Iz*r) + r*(-Iyx*p + Iy*q - Iyz*r))/Ix;
%qdot = (My + );
%rodt = (Mz + );

% Planar Symetric Body Frame rotational accelerations
% Ixy = Iyx = Iyz = Izy = 0.
%pdot = 
%qdot = 
%rdot = 

% Axially Symmetric Body Frame rotational accelerations
% Ixy = Ixz = Iyx = Iyz = Izx = Izy = 0.
pdot = (Mx + (Iy-Iz)*q*r) / Ix;
qdot = (My + (Iz-Ix)*p*r) / Iy;
rdot = (Mz + (Ix-Iy)*p*q) / Iz;


pedot = 0;
qedot = 0;
redot = 0;

                        
% quaternian derivatives
edots = 	0.5 * ...
   [	 0  p  q  r;
   	-p  0  r -q;
      -q -r  0  p;
      -r  q -p  0 ] * [ e0; e1; e2; e3 ];
      
e0dot = edots(1);
e1dot = edots(2);
e2dot = edots(3);
e3dot = edots(4);
      
      
      
% RETURN VALUES

% velocities (Earth Frame)
ydot(1) = vxe;									
ydot(2) = vye;									
ydot(3) = vze;

% accelerations (Body Frame)
ydot(4) = udot;
ydot(5) = vdot;
ydot(6) = wdot;

% quaternians
ydot(7) = e0dot;
ydot(8) = e1dot;
ydot(9) = e2dot;
ydot(10) = e3dot;

% rotation accelerations (Body Frame)
ydot(11) = pdot;
ydot(12) = qdot;
ydot(13) = rdot;

e(index,1)=e0;
e(index,2)=e1;
e(index,3)=e2;
e(index,4)=e3;


% update data array
data(index,1) =  time;
data(index,2) =  dt;
data(index,5) =  index;
data(index,10) = x;					% Earth Frame position
data(index,11) = y;
data(index,12) = z;
data(index,13) = vxe;				% Earth Frame velocities
data(index,14) = vye;
data(index,15) = vze;
data(index,16) = u;					% Body Frame velocities
data(index,17) = v;
data(index,18) = w;
data(index,19) = vxedot;			% Earth Frame accelerations
data(index,20) = vyedot;
data(index,21) = vzedot;
data(index,22) = udot;				% Body Frame accelerations
data(index,23) = vdot;
data(index,24) = wdot;
data(index,25) = phi;				% Body Frame orientations
data(index,26) = theta;
data(index,27) = psi;
data(index,28) = e0;					% Body Frame orientations
data(index,29) = e1;
data(index,30) = e2;
data(index,31) = e3;
data(index,32) = pe;					% Earth Frame angular velocities
data(index,33) = qe;
data(index,34) = re;
data(index,35) = p;					% Body Frame angular velocities
data(index,36) = q;
data(index,37) = r;
data(index,38) = pedot;				% Earth Frame angular accelerations
data(index,39) = qedot;
data(index,40) = redot;
data(index,41) = pdot;				% Body Frame angular accelerations
data(index,42) = qdot;
data(index,43) = rdot;
data(index,45) = inert_mass;




index = index + 1;

ydot = ydot';