clc; 
clear;
xdel(winsid());
// ------------------------- PARAMETERS --------------------------
m=0.039                  // Mass                     [kg]
tsim=5                   // Time of simulation       [s]
rho=1000                 // Water denisty            [kg/m^3]
D=2e-3                   // Drag constant            [N/(m/s)^2] (ec 10)
Ae=0.00038               // Cross-secctional area of exhaust nozzle [m^2]
Vt=0.002                 // WaterRocket total Volume [m^3]
Pa=101325                // Atmospheric preassure    [Pa]
g=9.81                   // Gravity                  [m/s^2]
tstep=0.01               // Time step                [s]
t=0:tstep:tsim
// ---------------------- INITIAL VALUES ------------------------
y0=0; v0=0;              // Initial position and velocity
t0=0;                    // Initial time             [s]
P0=4*Pa;                 // Initial Pressure         [Pa]
//V0=Vt*2/3;               // Initial Air Volume
PY0=[P0;y0;v0];

function Ymax=costf(V)
    V0=V;                    // Initial Air Volume
    Pbo=P0*V0/Vt;            // from ec. 5 (Pressure at which reocket is empty)
    PY=ode(PY0,t0,t,dotf)
//    Ymax=-max(PY(2,:));
    Ymax=1/max(PY(2,:));
//    printf("\nV=%0.3fl\tYmax=%0.3fm",V*1000,-Ymax);
endfunction

function dotPY=dotf(t,PY)
    P=PY(1);
    y=PY(2);
    v=PY(3);

    if P>=Pa then
      dP=-P^2/(P0*V0)*Ae*sqrt(2*(P-Pa)/rho); // ec. 7
    else dP=0;
    end;
    T=2*(P-Pa)*Ae              // Trust force ec. 4

    if P>=Pbo  then            // while there is still water inside 
      M=rho*(Vt-P0*V0/P)+m;                  // ec. 15
    else                       // when rocket is empty of water
      M=m;
      T=0;
    end

    Fdrag=D*v*abs(v);         // ec. 10
    dy =v;                    // ec. 16
    dv =(T-Fdrag)/M-g;        // ec. 17
 
    dotPY=[dP;dy;dv]
endfunction

function result=myfun(x, optimValues, state);
    it=optimValues.iteration;
    fv=optimValues.fval;
    result(it+1,1:2)=[it+1,fv];
endfunction
//------------------------- MAIN  --------------------------
//opt = optimset ( "Display" , "iter","OutputFcn",myfun,"TolFun",1.e-5 );
opt = optimset ( "Display" , "iter","PlotFcns",optimplotfval,"TolFun",1.e-8 );//Para
//opt = optimset ( "Display" , "iter");//Para ver pasos de optim
[Voptimaire,Ymax]=fminsearch(costf,0.0006,opt);
V0=Voptimaire;
Pbo=P0*V0/Vt;
Voptimagua=(Vt-Voptimaire)*1000;//Optimal initial water volume in liters
printf("\nLa altura máxima que alcanza el cohete es de %f m\n",1/Ymax)
printf("Se logra con un volumen inicial de agua de %f l\n",Voptimagua)
PY=ode(PY0,t0,t,dotf);

Vaire=0.01*Vt:0.00005:1*Vt;
Vagua=Vt-Vaire;
for i=1:length(Vaire)
    Y(i)=1/costf(Vaire(i));
end
//-------------------- GRAPHS ----------------------------
FigureSize=[1200,1000];
clf(1); scf(1);
    fig=gcf();
    fig.figure_size=FigureSize;
    fig.figure_name="Presión";

    ax=gca();
    ax.font_size=3.5;
    xgrid(18,1,9);
//    plot(t(1:1/tstep),PY(1,1:1/tstep)/1000,"thickness",3);
    plot(t(1:3:1/tstep),PY(1,1:3:1/tstep)/1000,"*","thickness",3);
    plot([0,1],[Pbo/1000,Pbo/1000],"k","thickness",2);
    xlabel("Tiempo [s]","fontsize",4.5);
    ylabel("Presión [kPa]","fontsize",4.5)
    legend("Presión al interior del cohete",...
           "Presión a la cual el cohete se queda sin agua");


clf(2); scf(2);
    fig=gcf();
    fig.figure_size=FigureSize;
    fig.figure_name="Altura";

    ax=gca();
    ax.font_size=3.5;
    xgrid(18,1,9);
//    plot(t,PY(2,:),"thickness",3);
    plot(t(1:10:$),PY(2,1:10:$),"*","thickness",3);
    xlabel("Tiempo [s]","fontsize",4.5);
    ylabel("Altura [m]","fontsize",4.5);
    
clf(3); scf(3);
    fig=gcf();
    fig.figure_size=FigureSize;
    fig.figure_name="Velocidad";
    ax=gca();
    ax.font_size=3.5;
    xgrid(18,1,9);
    plot(t,PY(3,:),"thickness",3);
    xlabel("Tiempo [s]","fontsize",4.5);
    ylabel("Velocidad [m/s]","fontsize",4.5)
    
clf(4); scf(4);
    fig=gcf();
    fig.figure_size=FigureSize;
    fig.figure_name="Altura Max";        
    ax=gca();
    ax.font_size=3.5;
    xgrid(18,1,9);
    plot(Vagua'*1000,Y,"*","thickness",3);
    xlabel("Volumen inicial de agua [l]","fontsize",4.5);
    ylabel("Altura máxima [m]","fontsize",4.5)