close all, clc

%Physical Parameters
h1 = 70;  %m
h2 = 25;  %m
h3 = 10;  %m
d = 3.66;  %m
rho = .01;  %kg/m^3
sigma = .7;
V = 890;  %m/s
Ru = 8314.4589;  %J/kg*K
Tw = 300;  %K
MW = 28.97;  %kg/kmol
J = 37576000;  %kg*m^2
T = 620000;  %N

K1 = (h3*T)/J;
K2 = ((-h1*h2*d*sigma*rho*V)*sqrt((pi()*Ru*Tw)/(2*MW)))/J;


%Step input parameters
t_start_step = 0;  %step time (sec)
step_initial_val = 0;  %value before step (deg.)
step_final_val = 1;  %value after step (deg.)

%Controller gains
Kp = 5;
Kd = 3;
Ki = .5*((Kd*Kp*h3*T)/J);


G3 = tf([h3*T*Kd h3*T*Kp h3*T*Ki],[J h3*T*Kd h3*T*Kp h3*T*Ki]); %Closed Loop PID

[Y,t]=step(1*G3);


plot(linear_step.Time,linear_step.Data), hold on
plot(nl_step1.Time,nl_step1.Data);
plot(nl_step2.Time,nl_step2.Data);
legend('Linear','Nonlinear Case 1','Nonlinear Case 2')
xlabel('Time (sec)')
ylabel('Theta (deg.)')
title('Comparison Between Linear and Nonlinear Systems')

figure(2)
plot(staircase_output.Time,staircase_output.Data,'linewidth',1.5)
xlabel('Time (sec)')
ylabel('Theta (deg.)')
title('Vehicle Response: Rocket Commanded to 10 deg. AOA')

figure(3)
plot(staircase_controller_effort.Time,staircase_controller_effort.Data)
xlabel('Time (sec)')
ylabel('Alpha (deg.)')
title('TVC Angle, Staircase Input (Controller Effort)')

figure(4)
plot(linear_step_angular_vel.Time,linear_step_angular_vel.Data), hold on
plot(nl_step1_angular_vel.Time,nl_step1_angular_vel.Data)
plot(nl_step2_angular_vel.Time,nl_step2_angular_vel.Data)
xlabel('Time (sec)')
ylabel('Rotation Rate (deg/sec)')
title('Vehicle Rotation Rates')
legend('Linear','Nonlinear Case 1','Nonlinear Case 2')

figure(5)
plot(staircase_input.Time,staircase_input.Data,'linewidth',1.5)
xlabel('Time (sec)')
ylabel('Commanded angle, theta_desired (deg.)')
title('Staircase Input Signal')


figure(6)
plot(ramp_input.Time,ramp_input.Data,'linewidth',1.5), hold on
plot(ramp_output.Time,ramp_output.Data,'linewidth',1.5), hold on
xlabel('Time (sec)')
ylabel('Theta_desired and Theta (deg.)')
title('Vehicle Response: Commanded to 15 deg. Using Ramp')
legend('Input', 'Response','Location','Northwest')

figure(7)
plot(ramp_controller_effort.Time,ramp_controller_effort.Data,'linewidth',1.5)
xlabel('Time (sec)')
ylabel('Alpha (deg.)')
title('TVC Angle, Ramp Input (Controller Effort)')

figure(8)
plot(ramp_angular_vel.Time,ramp_angular_vel.Data,'linewidth',1.5)
xlabel('Time (sec)')
ylabel('Rotation Rate (deg/s)')
title('Vehicle Rotation Rate, Ramp Input')