// ~~~~ ANALISE DOS CENARIOS ~~~~ 

clear

// PARÂMETROS
g = 9.81; // [m/s2]
Mt = 0.82; // [kg]
Iy = 8.1*(10^-3); // [kgm2]
Ce = 0.1154;
l = 0.29; // [m]

// VARIAVEIS
w1 = 1; // velocidade angular do rotor 1 [rad/s]
w3 = 1; // velocidade angular do rotor 3 [rad/s]

// TEMPO
t0 = 0; //tempo inicial [s]
dt = 0.01; //passo de integração [s]
tspan = 6; //intervalo de integração [s]
t = t0:dt:tspan; //vetor linha do tempo [s]

// CONDICOES INICIAIS DA TRAJETORIA NOMINAL (EQUILÍBRIO)
xn = 0;
xpn = 0;
zn = 0.3; // altura da base do drone ( 0.3 m)
zpn = 0;
thetan = 0; 
thetapn = 0;

Xn0 = [xn; xpn; zn; zpn; thetan; thetapn]; // vetor de condições iniciais da trajetória nominal

// CONDIÇÕES INICIAIS PARA O REFERENCIAL MÓVEL
x0 = 0; // posição x [m]
xp0 = 0; // velocidade em x [m/s]
z0 = 0; // posição em z [m]
zp0 = 0; // velocidade em z [m/s]
theta0 = 0.01; // posição em torno de y [rad]
thetap0 = 0; // velocidade em torno de y [rad/s]

X0 = [x0; xp0; z0; zp0; theta0; thetap0]; // vetor das condições iniciais

// ENTRADAS
u1 = Mt*g/(cos(theta0)); // empuxo
u3 = Ce*l*(w3^2 - w1^2); // momento de arfagem

U = [u1;u3]; // vetor de entrada

Un = [Mt*g;0]; // vetor de entrada nominais

//ESPAÇO DE ESTADOS

//sistema não linear
funcprot(0);
    function dy = VANT(t,y)
        dy(1) = y(2);
        dy(2) = (1/Mt)*sin(y(5))*u1;
        dy(3) = y(4);
        dy(4) = - g +((1/Mt)*cos(y(5))*u1);
        dy(5) = y(6);
        dy(6) = (1/Iy)*u3;
    endfunction;

// sistema linear
A=[0,1,0,0,0,0;..
   0,0,0,0,g,0;...
   0,0,0,1,0,0;..
   0,0,0,0,0,0;..
   0,0,0,0,0,1;..
   0,0,0,0,0,0]
   
B=[0,0;..
   0,0;..
   0,0;..
   (1/Mt),0;..
   0,0;..
   0,(1/Iy)]
   
C=[1,0,0,0,0,0;..
   0,1,0,0,0,0;..
   0,0,1,0,0,0;..
   0,0,0,1,0,0;..
   0,0,0,0,1,0;..
   0,0,0,0,0,1]
   
D=[0,0;..
   0,0;..
   0,0;..
   0,0;..
   0,0;..
   0,0]

VANTlin = syslin('c',A,B,C,D); // definição do sistema linear

deltaU = (U-Un)*ones(t);
deltaX0 = X0 - Xn0; 

// ~~ SIMULAÇÕES ~~

// sistema não linear 
X = ode(X0,t0,t,VANT); 

// sistema linear
deltaXlin = csim(deltaU,t,VANTlin,deltaX0);

// ~~ RESULTADOS ~~

// REFERENCIAL MÓVEL

//sistema não linear
xp = X(2,:) // velocidade em x [m/s]
zp = X(4,:) // velocidade em z [m/s]
theta = X(5,:) // deslocamento angular em torno de y [rad]
thetap = X(6,:) // velocidade de arfagem (em torno de y) [rad/s]

//sistema linearizado
xp_lin = deltaXlin(2,:) // velocidade em x [m/s]
zp_lin = deltaXlin(4,:) // velocidade em z [m/s]
theta_lin = deltaXlin(5,:) // deslocamento angular em torno de y [rad]
thetap_lin = deltaXlin(6,:) // velocidade de arfagem (em torno de y) [rad/s]

// MUDANÇA PARA O REFERENCIAL FIXO

//sistema não linear
xpInercial = cos(theta).*xp + sin(theta).*zp // velocidade em x no referencial fixo [m/s]
zpInercial = - sin(theta).*xp + cos(theta).*zp // velocidade em z no referencial fixo [m/s]

//sistema linearizado
xpInercial_lin = cos(theta_lin).*xp_lin + sin(theta_lin).*zp_lin // velocidade em x no referencial fixo [m/s]
zpInercial_lin = -sin(theta_lin).*xp_lin + cos(theta_lin).*zp_lin // velocidade em z no referencial fixo [m/s]

//velocidades nominais
xpnInercial = 0;
zpnInercial = 0;

// INTEGRAÇÃO

function F = integra(f, t, F0) 
    F = zeros(f) 
    F(1) = F0//definição da condição inicial
    for i = 2:1:length(f)
        F(i) = inttrap(t(1:i),f(1:i)) // integração pelo método dos trapézios
    end
endfunction

//sistema não linear
xInercial = integra(xpInercial,t,0) // posição x no referencial fixo [m]
zInercial = integra(zpInercial,t,0) + 0.3 // posição z no referencial fixo [m]
thetaInercial = theta // posição angular em torno de y [rad]

//sistema linearizado
xInercial_lin = integra(xpInercial_lin,t,0) // posição x no referencial fixo [m]
zInercial_lin = integra(zpInercial_lin,t,0) + 0.3//posição z no referencial fixo [m]
thetaInercial_lin = theta_lin // posição angular em torno de y [rad]

//deslocamento nominal
xnInercial = xn // posição x no SC inercial [m]
znInercial = zn // posição z no SC inercial [m]
thetanInercial = thetan // posição angular (em torno de y) no referencial fixo[rad]

//~~ PLOTAGENS ~~

xset('window',1)
plot2d(t,[xpInercial',xpInercial_lin',xpn*ones(t)'])//u' = u transposto
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Velocidade\: em\: X$','$\Large t\: [s]$','$\Large \dot X_E\: [m/s]$');
xgrid(1)

xset('window',2)
plot2d(t,[xInercial',xInercial_lin',xnInercial*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Deslocamento\: em\: X$','$\Large t\: [s]$','$\Large X_E\: [m]$')
xgrid(1)

xset('window',3)
plot2d(t,[zpInercial',zpInercial_lin',zpn*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Velocidade\: em\: Z$','$\Large t\: [s]$','$\Large \dot Z_E\: [m/s]$')
xgrid(1)

xset('window',4)
plot2d(t,[zInercial',zInercial_lin',znInercial*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Deslocamento\: em\: Z $','$\Large t\: [s]$','$\Large Z_E\: [m]$')
xgrid(1)

xset('window',5)
plot2d(t,[thetap',thetap_lin',thetapn*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Velocidade\: de\: Arfagem$','$\Large t\: [s]$','$\Large \dot \theta_E\: [rad/s]$')
xgrid(1)

xset('window',6)
plot2d(t,[thetaInercial',thetaInercial_lin',thetanInercial*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Ângulo\: de\: Arfagem$','$\Large t\: [s]$','$\Large \theta_E\: [rad]$')
xgrid(1)

xset('window',7)
plot2d([xInercial',xInercial_lin',xnInercial*ones(t)'],[zInercial',zInercial_lin',znInercial*ones(t)'])
e = gce();
e.children(2).thickness=2;
e.children(3).thickness=2;
e.children(2).foreground=2;
e.children(3).foreground=5;
e.children(1).thickness=2;
e.children(1).line_style=3;
e.children(1).foreground=3;
hl = captions(e.children,['Não linear';'Linearizado';'Trajetória nominal']);
xtitle('$\LARGE Trajetória\: (Z-X)$','$\Large X_E\: [m]$','$\Large Z_E\: [m]$')
xgrid(1)