close all; clc; clear; format long; fclose('all');

% Definovani promennych
XX=[];
VYS=[];
dt=0.1;
G = 6.67e-11;       % Gravitacni konstanta
ME = 5.97e24;       % Hmotnost Zeme
RE = 6.37e6;        % Radius Zeme

% lze upravovat
Isp0_1=175;         % Delka horeni 1. stupne
T_1 = 327000;       % Tah 1. stupne
A_1=(1.62/2)^2*pi;  % Plocha trysky 1. stupne
S_1=(1.8/2)^2*pi;   % Plocha rakety 1. stupne
mr_1=3150;          % vaha rakety 1. stupen
mp0_1=19450;        % Puvodni vaha paliva 1. stupne
Isp0_2=360;         % Delka horeni 2. stupne
T_2 = 22241;        % Tah 2. stupne
A_2=(1.27/2)^2*pi;  % Plocha trysky 2. stupne
S_2=(1.5/2)^2*pi;   % Plocha rakety 2. stupne 
mp0_2=2750;         % Puvodni vaha paliva 2. stupne 
mr_2=250;           % vaha rakety 2. stupen

            %%%%%%%%%%%%%%%%%%%%% POSTUP %%%%%%%%%%%%%%%%%%%%%%
            %                                                 %
            %       1) y_0             - řádek 42             %
            %       2) letadlo         - řádek 43             %
            %       3) trajektorie     - řádek 44             %
            %       4) mr_pay          - řádek 45             %
            %       5) alpha0          - řádek 46-51          %
            %       6) mp0             - řádek 52             %
            %       7) proc            - řádek 53             %
            %                                                 % 
            %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
            % další proměnné - yk, delta, mr, A, S, T, vx a Isp

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% UPRAVIT PRED SIMULACI %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
yk=200000;                          % finální výška oběhu
delta=90;                           % po volnem padu z letadla, rozdil mezi trajektorii a tahem

for y_0=0:4000:40000                % počáteční výška vypuštění - balon 30000; letadlo 10668; země 0;
letadlo=0;                          % z letadla => 1;      z balonu/zeme => 0;
trajektorie=0;                      % konst. uhel => 1;    parabola => 0;
mr_pay=250;                          % vaha satelitu
%OK for alpha0 = 25:2:50               % pro konst uhel, letadlo, mp0=20000:23000; proc=88:93; y_0=10668; alpha0=10:90
%OK for alpha0 = 6:2:50               % pro konst uhel, balon, mp0=28000:32000; proc=89:93; y_0=30000; alpha0=6:90
%OK for alpha0 = 60:90              % pro parabolický z letadla, mp0=18000:24000; proc=85:90; y_0=10668; alpha0=66:90 
for alpha0 = 89.998:0.0001:90  % pro parabolický z balonu, mp0=16000:20000; proc=82:86; y_0=30000; alpha0=89.9982:90
%OK for alpha0 = 89.9999997:0.00000001:90   % pro parabolický ze země, mp0=26000:30000; proc=87:88; y_0=0; alpha0=89.9999997:90
%OK for alpha0 = 20:90              % pro parabolický z balonu, mp0=16000:20000; proc=82:86; y_0=30000; alpha0=89.9982:90
for mp0=16000:2000:20000            % celková váha paliva rakety
    for proc=86:88                  % procentuelní rozložení paliva do stupně 1 a 2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% UPRAVIT PRED SIMULACI %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        mp_1=mp0*proc/100;
        mp_2=mp0*(1-proc/100);
        Isp_1=Isp0_1/mp0_1*mp_1;
        Isp_2=Isp0_2/mp0_2*mp_2;
        mp=mp_1+mp_2;
        mr=mr_1+mr_2+mr_pay; %vaha rakety u startu
        m = mr + mp;
        F_T=0; %pocatecni sila tahu
        F_D=0; %pocatecni sila odporu
        y=y_0; %pocatecni vyska
        x=0; %pocatecni vzdalenost
        if letadlo==1
              vx=200;
        else
              vx=0;
        end
        vy=0; %pocatecni rychlost vertikalni
        v=sqrt(vx^2+vy^2); %pocatecni rychlost
        alpha_T=0;
        alpha_D=alpha_T;
        t=0; %cas vzletu cas
        t=t+dt; % krok
        ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;

        DIPLOMKAdata = fopen('Rocket.dat','w'); % vytvori a otevre slozku pro ukladani dat
        fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x); % Postupne vkladani promennych do souboru

if letadlo==1
    drop=4; %4 sekundy volného pádu, poté zážeh
  while t<drop % bez tahu 4 sekundy
       [~,~,rho,c] = USSA(y);
       [cd]=Drag_coef(v,c);
       D=(cd*rho*S_1*v^2)/2;
       F_T=0;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
       alpha_D=atand(dy/dx);
       ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;
       v=sqrt(vx^2+vy^2);
       fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x);
       t=t+dt;  
  end
       alpha_T=alpha_D+delta;
   
   while alpha_D<=alpha0 && mp>mp_2 %tah rakety pod alpha stupni dokud nenaletne uhel vzletu alpha0
       [~,Po,rho,c] = USSA(y);
       [cd]=Drag_coef(v,c);
       D=(cd*rho*S_1*v^2)/2;
       mp=mp-dt*mp_1/Isp_1;
       m=mr+mp;
       F_T=T_1-Po*1000*A_1;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
       alpha_D=atand(dy/dx);
       alpha_T=alpha_D+delta;
       if alpha_T>=90 
          alpha_T=90;
       end
       ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;
       v=sqrt(vx^2+vy^2);
       fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x);
       t=t+dt;
   end

elseif letadlo==0 
    drop=0;
end

alpha_T=alpha0;
alpha_D=alpha0;

while t<=drop+Isp_1 && mp>mp_2 % az do konce paliva 1. stupne
       [~,Po,rho,c] = USSA(y);
       [cd]=Drag_coef(v,c);
       D=(cd*rho*S_1*v^2)/2;
       vk = sqrt(G*ME/(RE+y));  % 1. kosmicka rychlost          
      if  y<yk-(vy^2/(2*(G*ME/(RE+y)^2)))
       mp=mp-dt*mp_1/Isp_1;
       m=mr+mp;
       F_T=T_1-Po*1000*A_1;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
       alpha_D=atand(dy/dx);
       dalpha=abs(alpha_D-alpha0);
       if trajektorie == 1
           alpha_T=acosd((ax*m-F_D*cosd(alpha0))/F_T);
           if alpha_D>alpha0
               alpha_T=alpha_T-dalpha/2;
           else 
               alpha_T=alpha_T+1.1*dalpha;
           end
       if alpha_T>90
           alpha_T=90;
       end
       elseif trajektorie == 0
           alpha_T=atand(dy/dx);
       end       
      else
           if vy>0
               alpha_T=0;
               alpha_D=atand(dy/dx);
           else
               alpha_D=atand(dy/dx);
               alpha_T=asind((m*((G*ME/(RE+y)^2)-vx^2/(RE+y))-F_D*sind(alpha_D))/F_T);
           end
       mp=mp-dt*mp_1/Isp_1;
       m=mr+mp;
       F_T=T_1-Po*1000*A_1;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
      end
        ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;
        v=sqrt(vx^2+vy^2);
        fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x);
        t=t+dt;
 end % ukonceni tahu stupne 1

       mr=mr-mr_1; % odpoutani stupne 1
       m=mr+mp;

    while t<=drop+Isp_1+5 % bez tahu 5 sekund
       [~,~,rho,c] = USSA(y);
       [cd]=Drag_coef(v,c);
       D=(cd*rho*S_1*v^2)/2;
       F_T=0;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
       alpha_D=atand(dy/dx);
       ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;
       v=sqrt(vx^2+vy^2);
       fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x);
       t=t+dt;  
   end
        
   while t<drop+Isp_1+5+Isp_2 && mp>0 % zazeh stupne 2, tah na Isp_2 sekund
       [~,Po,rho,c] = USSA(y);
       [cd]=Drag_coef(v,c);
       D=(cd*rho*S_2*v^2)/2;
       vk = sqrt(G*ME/(RE+y));  % 1. kosmicka rychlost          
      if  y<yk-(vy^2/(2*(G*ME/(RE+y)^2)))
       mp=mp-dt*mp_2/Isp_2;
       m=mr+mp;
       F_T=T_2-Po*1000*A_2;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
       alpha_D=atand(dy/dx);
       dalpha=abs(alpha_D-alpha0);
       if trajektorie == 1
           alpha_T=acosd((ax*m-F_D*cosd(alpha0))/F_T);
           if alpha_D>alpha0
               alpha_T=alpha_T-dalpha/2;
           else 
               alpha_T=alpha_T+1.1*dalpha;
           end
       if alpha_T>90
           alpha_T=90;
       end
       elseif trajektorie == 0
           alpha_T=atand(dy/dx);
       end       
      else
           if vy>0
               alpha_T=0;
               alpha_D=atand(dy/dx);
           else
               alpha_D=atand(dy/dx);
               alpha_T=asind((m*((G*ME/(RE+y)^2)-vx^2/(RE+y))-F_D*sind(alpha_D))/F_T);
           end
       mp=mp-dt*mp_2/Isp_2;
       m=mr+mp;
       F_T=T_2-Po*1000*A_2;
       Fx_T=F_T*cosd(alpha_T);
       Fy_T=F_T*sind(alpha_T);
       F_D=-D;
       Fx_D=F_D*cosd(alpha_D);
       Fy_D=F_D*sind(alpha_D);
       ax=(Fx_T+Fx_D)/m;
       ay=(Fy_T+Fy_D)/m+(vx+0)^2/(RE+y)-(G*ME/(RE+y)^2);
       vx=vx+ax*dt;
       vy=vy+ay*dt;
       x0=x;
       x=x+vx*dt;
       dx=x-x0;
       y0=y;
       y=y+vy*dt;
       dy=y-y0;
      end
        ar=(F_T+F_D*cosd(abs(alpha_T-alpha_D)))/m;
        v=sqrt(vx^2+vy^2);
        fprintf(DIPLOMKAdata,'%f %f %f %f %f %f\n',t,y,v,ar,m,x);
        t=t+dt;
   end % ukonceni tahu stupne 2

       mr=mr-mr_2; % odpoutani stupne 2
       m=mr+mp;

if (x>=0) && (alpha_D<=5) && (y>=yk) %plynule navazani na orbitu

       fclose(DIPLOMKAdata); % ukonci vkladani promennych do slozky
       load Rocket.dat % nacte slozku pro vykresleni grafu

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GRAF %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if all(isreal(v)==1 & isreal(y)==1 & any(Rocket(:,4)<100) & any(Rocket(:,4)>-50) & y>=yk & v<9000) %#ok<COMPNOP>
    if isempty(VYS==1)
        VYS=[mp0,alpha0,v,proc,y_0];
    else
        if y_0==VYS(end,5)
            if v>VYS(end,3)
                VYS(end,:)=[mp0,alpha0,v,proc,y_0];
            end
        else
            VYS=[VYS;[mp0,alpha0,v,proc,y_0]]; %#ok<AGROW>
        end
    end
end
end
    end
end
end
end

if trajektorie==0
    tra=('Parabolicky');
elseif trajektorie == 1
    tra=('Konstantní úhel');
end
if letadlo == 1
    let=('Letadlo');
else
    let=('Balon');
end

fig=figure('Name','Graf závislosti výsledné rychlosti na počáteční výšce vzletu');
plot(VYS(:,5)/1000,VYS(:,3))
title('Graf závislosti výsledné rychlosti na počáteční výšce vzletu')
ylabel('Rychlost (m/s)')
xlabel('Vyska (km)')
beep
writematrix(VYS,['Tabulka Vyska ',tra,' ',let,'.xlsx'])
saveas(fig,['Vyska ',tra,' ',let,'.png'])