clear all; clc;
%% Create Thrust Curve using Piecewise Function
dt = 0.001;

t1 = 0.148 : dt : 0.419;
t2 = 0.420 : dt : 3.382;
t3 = 3.383 : dt : 3.45;

y1 = 65.165.*t1 - 2.3921;
y2 = 0.8932*t2.^6 - 11.609*t2.^5 + 60.739*t2.^4 - 162.99*t2.^3 ...
    + 235.6*t2.^2 - 174.43*t2 + 67.17;
y3 = -195.78*t3 + 675.11;

m = 1.2;

a = [y1, y2, y3] ./ m;
t = 0.148 : dt : 3.45;
thrustCurve = [t', (a*m)'];

%% Integrate
v = 0;
h = 0;
g = -9.81;
a = a + g;

for i = 1 : length(a)
    v = ((a(i)) * dt) + v;
    h = v * dt + h;
end

h = h + ((v^2) / (2*-g))
vb = v

thrustFiring = 0;
dt = 0.001;
v = 0;
j = 1;
k = 1;
t = 0;

while 1
    if abs(v + vb) < 0.01
        thrustFiring = 1;
        tb = 0;
    end
    
    if thrustFiring == 0
        v = g*dt + v;
        h = v*dt + h;
        a = g;
    elseif thrustFiring == 1
        if ((tb >= 0.148) && (tb <= 3.45))
            v = v + a(j)*dt;
            h = h + v*dt;
            
            disp(tb)
            j = j + 1;
        else
            v = g*dt + v;
            h = v*dt + h;
        end
        
        tb = tb + dt;
    end
    accel(k) = a;
    vz(k) = v;
    z(k) = h;
    time(k) = t;

    t = t + dt;
    k = k + 1;
    if h < 0
        break
    end
end

figure(1)
plot(time, z)
figure(2)
plot(time, vz)
figure(3)
plot(time, accel)