clear all
close all

Fi=15000; %initial thrust [N]
Isp=2756.6*0.9; %specific impulse [Ns/kg]
OF(1)=2.5; %initial oxidizer to fuel ratio
mp_doti=Fi/Isp; %initial propellants mass flow rate [kg/s]
mo_dot=mp_doti-mp_doti/(OF(1)+1); %oxidizer mass flow rate [kg/s]
rhof=920;
R(1)=5.5; %initial radius [cm]
dt=0.1; %time step [s]
tb=48; %burn time [s]
t=(0:dt:tb); %burning time
Go(1)=mo_dot*1000/(pi*R(1)^2);

r_dot(1)=0.163*Go(1)^0.62/(((1+1/OF(1))^0.38-1)*OF(1)); %regression rate [mm/s]
mf_dot(1)=mo_dot/OF(1);
L=mf_dot(1)/(2*pi*R(1)/100*920*r_dot(1)/1000); %fuel grain length [m]

for i=2:length(t)
    R(i)=R(i-1)+r_dot(i-1)/10*dt;
    Go(i)=mo_dot*1000/(pi*R(i)^2);
    r_dot(i)=0.163*Go(i)^0.62/(((1+1/OF(i-1))^0.38-1)*OF(i-1));
    mf_dot(i)=2*pi*R(i)/100*L*rhof*r_dot(i)/1000; %fuel mass flow rate [kg/s]
    OF(i)=mo_dot/mf_dot(i);
end
OFav1=mean(OF)
rav=mean(r_dot)
Goav=mean(Go) 
mp_dot=mo_dot+mf_dot; %propellants mass flow rate
mpav=mean(mp_dot)
mo=mo_dot*tb; %oxidizer mass
mf(1)=0;
for i=2:length(mf_dot)
    mf_doti(i-1)=(mf_dot(i-1)+mf_dot(i))/2;
    mf(i)=mf(i-1)+mf_doti(i-1)*dt; %fuel mass
end
mo=mo_dot*t; %total oxidizer weight needed [kg]
mp=flip(mf)+flip(mo); %propellants weight [kg]
me=0.7*mp(1); %empty weight
F=mp_dot*Isp;
Fg=(mp+me)*9.81;
Fav=mean(F)
TWR = F./Fg;
r=0.488.*Go.^0.62;

v(1)=0; %initial velocity [m/s]
h(1)=0; %initial height [m]
FD(1)=0;
accel(1)=(F(1)-Fg(1)-FD(1))/(mp(1)+me);
rho(1)=1.225; %air density at sea level [kg/m3]
Cd=0.5;
Area=pi*(R(end)+3)^2*10^(-4); %cross section area [m2]

for i = 2:length(t)
    v(i) = v(i-1) + accel(i-1)*dt;
    h(i) = h(i-1) + v(i)*dt;
    if h(i)<=11000
        rho(i)=rho(1)*(1-h(i)/44308)^4.2553;
    elseif  h(i) <=20000
        rho(i)=0.363798*exp(-9.807*(h(i)-11000)/(287.1*216.65));
    else
        rho(i)=0.088022*(1+(h(i)/1000-20)/216.65)^(-9.807/0.287+1);
    end
    FD(i)=0.5*rho(i)*v(i)^2*Cd*Area;
    accel(i)=(F(i)-Fg(i)-FD(i))/(mp(i)+me);
end
v_out(1)=v(end)
h_out(1)=h(end)
FD_out(1)=FD(end);
a_out(1)= -(me*9.81+FD_out(1))/me;
rho_out(1)=rho(end);
i=1;
t_out(1)=tb;
while v_out(i) > 0
    i=i+1;
    v_out(i) = v_out(i-1) + a_out(i-1)*dt;
    h_out(i) = h_out(i-1) + v_out(i-1)*dt;
    rho_out(i)=0.088022*(1+(h_out(i)/1000-20)/216.65)^(-9.807/0.287+1);
    FD_out(i)=0.5*rho_out(i)*v_out(i)^2*Cd*Area;
    a_out(i)=-(me*9.81+FD_out(i-1))/me;
    t_out(i)=t_out(i-1)+dt;
end
h_out(end)



figure(1)
plot(t,r_dot);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("$\dot{r}$ [mm/s]",'Interpreter','latex')
ylim([0 6])

figure(2)
plot(t,R);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("R [cm]",'Interpreter','latex')

figure(3)
plot(t,Go);
grid
title("$G_o$(t)",'Interpreter','latex')
xlabel("t [s]",'Interpreter','latex')
ylabel("$G_o$ [g/cm$^2$/s]",'Interpreter','latex')

figure(4)
plot(Go,r_dot);
grid
title("$\dot{r}$($G_o$)",'Interpreter','latex')
xlabel("$G_o$ [g/cm$^2$/s]",'Interpreter','latex')
ylabel("$\dot{r}$ [mm/s]",'Interpreter','latex')
hold on
plot(Go,r);
hold off

figure(5)
plot(t,OF);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("O/F [-]",'Interpreter','latex')

figure(6)
plot(t,mf_dot);
grid
title("$\dot{m}_f$(t)",'Interpreter','latex')
xlabel("t [s]",'Interpreter','latex')
ylabel("$\dot{m}_f$ [kg/s]",'Interpreter','latex')

figure(7)
plot(t,F/1000);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("F [kN]",'Interpreter','latex')
ylim([13 15])

figure(8)
plot(t,h/1000);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("h [km]",'Interpreter','latex')
hold on
plot(t_out,h_out/1000);
hold off

figure(9)
plot(t,accel/9.81);
grid
xlabel("t [s]",'Interpreter','latex')
ylabel("a [g]",'Interpreter','latex')
hold on
plot(t_out,(a_out+9.81)/9.81);
hold off

figure(10)
plot(t,v);
grid
title("v(t)",'Interpreter','latex')
xlabel("t [s]",'Interpreter','latex')
ylabel("v [m/s]",'Interpreter','latex')
hold on
plot(t_out,v_out);
hold off