clc
clear all

V=150.0;%Keas
CLalpha=0.2;%CL alpha
M=24000;% lbs
Sw=100;%ft^2
Iy=7.3e6;%slug*ft^2
epalpha=0.01;
lt=30;%ft
Ldeltaelev=1.0;%????
cw=8.0;%ft
ro=0.002486476;%slug/ft^3
Lalphas=0.1;%?????
CMalpha=-1.6;%1/rad
g=32.2;%ft/s^2


q=0.5*ro*(V*1.6878099)^2;


Zalpha = -339.0036;% ft/s^2 %-CLalpha*q*Sw/M;
Zalphap = -7.6658;%ft/s%-Lalphas*lt*epalpha/(M*V*1.6878099);
Ztetap = -7.4741;%ft/s%-Lalphas*lt/(M*V*1.6878099);
Zdelev = -18.3410;%ft/s^2%-Ldeltaelev/M;
Malpha = -1.6165;%1/s^2
Malphap = -0.1425;%1/s
Mtetap = -0.4038;%1/s
Mdelev = -1.2124;%1/s^2

delta = (V*1.6878099-Zalphap);

A=[Zalpha/delta 0 (V*1.6878099+Ztetap)/delta  
    0 0 1 
    Malpha Malphap Mtetap];

B=[Zdelev/delta
    0
    Mdelev];
C=[1 0 0
    0 1 0
    0 0 1];
D=[ 0 0 0]';

tfinal=15;
t1 = [0 0.4 2.4 2.8 200]; 

u1=[1*0.017453293 -14*0.017453293 -14*0.017453293 0 0];
y=0:0.1:tfinal;
inp=interp1(t1,u1,y,'linear');

figure;
plot(inp);
legend('input');

t=0:0.1:tfinal;

x0 = [0 0 0]; %initial condition of system states
[y,x] = lsim(A,B,C,D,inp,t,x0); %do the simulation

ap=diff(x(:,1));
ap(numel(ap)+1)=ap(numel(ap));
nz=1.0+V*1.6878099*(-ap(:)+x(:,3))/g;

figure;
plot(t,x,t,inp)
leg = legend('$\alpha$','$\theta$','$\dot{\theta}$','input');
set(leg, 'Interpreter', 'latex');
xlabel('time [s]');
ylabel('Degree [rad]');

figure
plot(t,nz)
leg1 = legend('$n_z$');
set(leg1, 'Interpreter', 'latex');
xlabel('time [s]');
h = ylabel('$n_z [g]$'); 
set(h, 'Interpreter', 'latex');