%% Two-Step Constant Thrust Model
% This model requires the following .m files
%    makelists.m which makes the lists of initial and final times, b values,
%    velocities, and positions for each stage.
%    velocity.m which calculates the velocity during thrust and descent
%    position.m which calculates the position during thrust and descent
%    velcoast.m whcih calculates the velocity between burnout and apogee
%    poscoast.m wich calculates the position between burnout and apogee
%% Initialize parameters
% For the numbering of parameters:
% 1 = pre-launch
% 2 = thrust 1
% 3 = thrust 2
% 4 = coast to apogee
% 5 = post-apogee

% Thrust & time lists
% E12 from Rocketreviews
tTlist(1) = 0.2; % burn time in seconds
Tlist(1) = 140; % Average thrust in Newtons
tTlist(2) = 1.1; % burn time in seconds
Tlist(2) = 131.73; % Average thrust in Newtons

% Rocket mass
% The mass burned during a thrust phase is assumed proportional to the
% total impulse, which assumes that v_exit of the exhaust is constant.
mRocket = 0.33355; % kg Topshot measured
mMotorTotal = 0.206; % kg E12 from Rocketreviews
mPropellant = 0.094; % kg E12 from Rocketreviews
mPropellant1 = mPropellant*tTlist(1)*Tlist(1)/(Tlist(1)*tTlist(1) + ...
    Tlist(2)*tTlist(2)); % kg Amount burned during thrust 1
mPropellant2 = mPropellant - mPropellant1; % kg Amount burned during thrust 2
mCase = mMotorTotal - mPropellant;
mlist(1) = mRocket + mMotorTotal; % Pre-launch
mlist(2) = mRocket + mCase + mPropellant2 + mPropellant1/2; % First burn
mlist(3) = mRocket + mCase + mPropellant2/2; % 2nd burn
mlist(4) = mRocket + mCase; % Coast to apogee
mlist(5) = mRocket + mCase; % Post-apogee, usually same as during coast

% CD, Ap, & \rho air
D = 0.04064; % m Topshot spec, not measured
Ap = pi*D^2/4; % m^2
Tabs = 292.6; % K
Pabs = 101325; % Pa
R = 8.314462618; % J/mol K
Mair = 29; % kg/kg-mol
CD = 0.5; % Airframe drag coefficient. Typical value 0.7
rho = Mair*Pabs/R/Tabs/1000; % kg/m^3
Dp = 0.4572; % m, 18 inch parachute is .4572
CDp = 1.4; % Parachute CD. Typical value is 1.4
App = pi*Dp^2/4; % Parachute projected area.

% t0, v0, x0
t0 = 0; % Usually the case
v0 = 0; % Usually the case
x0 = 0; % Set to 0 for AGL, launch site height for MSL

% Make the lists of change points in the piecewise continuous function
[t0list,vmaxlist,blist,v0list,x0list] = makelists(tTlist, Tlist, mlist, ...
    CD, Ap, rho, t0, v0, x0, CDp, App);

% Calculate the acceleration, velocity, and position versus time
t = -1:0.01:20; % The time array to use. Could use landing time t0list(6)
tpre = t(t<t0list(2));
t1 = t(t>=t0list(2) & t<t0list(3));
t2 = t(t>=t0list(3) & t<t0list(4));
tcoast = t(t>=t0list(4) & t<t0list(5));
tff = t(t>=t0list(5));

apre = 0*tpre;
vpre = apre + v0;
xpre = apre + x0;

v1 = velocity(v0list(2), vmaxlist(2), blist(2), t0list(2), t1);
v2 = velocity(v0list(3), vmaxlist(3), blist(3), t0list(3), t2);
vcoast = velcoast(v0list(4), vmaxlist(4), blist(4), t0list(4), tcoast);
vff = velocity(v0list(5), vmaxlist(5), blist(5), t0list(5), tff);
v = [vpre v1 v2 vcoast vff];

x1 = position(x0list(2), v0list(2), vmaxlist(2), blist(2), t0list(2), t1);
x2 = position(x0list(3), v0list(3), vmaxlist(3), blist(3), t0list(3), t2);
xcoast = poscoast(x0list(4), v0list(4), vmaxlist(4), blist(4), t0list(4), tcoast);
xff = position(x0list(5), v0list(5), vmaxlist(5), blist(5), t0list(5), tff);
x = [xpre x1 x2 xcoast xff];

a1 = blist(2)*(1-v1.*v1/vmaxlist(2)^2);
a2 = blist(3)*(1-v2.*v2/vmaxlist(3)^2);
acoast = -blist(4)*(1+vcoast.*vcoast/vmaxlist(4)^2);
aff = blist(5)*(1-vff.*vff/vmaxlist(5)^2);
a = [apre a1 a2 acoast aff];

yyaxis left
plot(t, x, 'g', 'LineWidth',2)
title('Acceleration, Velocity, and Position vs. time for 1D Model')
xlabel('Time (s)')
ylabel('Altitude (m)')
yyaxis right
plot (t, v, 'r', t, a, 'b', 'LineWidth',2)
ylabel('Acceleration (m/s^2), Velocity (m/s)')

tdelay = t0list(5) - t0list(4) % Time to apogee from burnout
tapogee = t0list(5) % Time to apogee from launch
apogee = x0list(5) % Altitude at apogee
timpact = t0list(6) % Time from launch to landing
vimpact = v0list(6) % Velocity at landing, usually terminal velocity