clear;

//Constantes
rho = 1.2;

//Parametros do parafoil e sistema
b = 1.85;
c = .95;
d = c/4.;
S = b*c;
Jx = 16.530561;
Jy = 16.431061;
Jz = 0.2294;
Jxz = 0.030;
J = [Jx 0. -Jxz; 0. Jy 0.; -Jxz 0. Jz];
I = inv(J);
Ix = I(1,1);
Iy = I(2,2);
Iz = I(3,3);
Ixz = I(1,3);

//Coeficientes característicos
Clphi = - 0.01;
Clp = -0.52;
Cldeltaa = 0.0021;
Cnr = -0.6;
Cndeltaa = 0.0010;


theta0 = -5*%pi/180;
Va = 20;

//Matrizes A B C D
A = [ 0 0 1 tan(theta0); 0 0 0 1/cos(theta0); rho*S*b*Va^2*Ix*Clphi/2 0 rho*S*b^2*Va*Ix*Clp/4 rho*S*b^2*Va*Ixz*Cnr/4; rho*S*b*Va^2*Ixz*Clphi/2 0 rho*S*b^2*Va*Ixz*Clp/4 rho*S*b^2*Va*Iz*Cnr/4];
B = [0 ; 0 ; rho*S*b*Va^2*(Cldeltaa*Ix+Cndeltaa*Ixz)/2/d ; rho*S*b*Va^2*(Cldeltaa*Ixz+Cndeltaa*Iz)/2/d];
C = eye(4,4);
D = [0]

//Vetor de tempo
step = 0.01;
tf = 10.;
t = 0:step:tf;

//Entrada do sistema
u = zeros(1,length(t));
f = 1.;
omega = 2*%pi*f;
u(1,:) = sin(omega*t);

//Condições iniciais
x0 = [0; %pi/6; 0; 0.];

//Solver
sys = syslin('c',A,B,C);
G = ss2tf(sys);
polos = spec(A);
disp('Polos do sistema');
disp(polos);
[y,x] = csim(u,t,sys,x0);


//Calculo da trajetória
origin = [0;0;0];
trajetoria = zeros(3,length(t));
trajetoria(:,1) = origin;
for i = 2:length(t)
    trajetoria(1,i) = trajetoria(1,i-1) + step*Va*cos(theta0)*cos(x(2,i-1));
    trajetoria(2,i) = trajetoria(2,i-1) + step*Va*cos(theta0)*sin(x(2,i-1));
    trajetoria(3,i) = trajetoria(3,i-1) + Va*sin(theta0)*step
end

//Plotagem das saídas (rodar somente após ja ter rodado o não linear - sem fechar as janelas gráficas)

scf(1);
plot2d(t,x(1,:)*180/%pi,5);
legend("Não-linear", "Linear", 4)
scf(2);
plot2d(t,x(2,:)*180/%pi,5);
legend("Não-linear", "Linear", 4)
scf(3);
plot2d(t,x(3,:)*180/%pi,5);
legend("Não-linear", "Linear", 4)
scf(4);
plot2d(t,x(4,:)*180/%pi,5);
legend("Não-linear", "Linear", 4)
scf(5);
bode(G(1,:));
scf(6);
bode(G(2,:));
scf(7);
bode(G(3,:));
scf(8);
bode(G(4,:));


//Metodo alternativo
function [x]=integrador3(A,B,C,D,u,t,x0)
    // Parametros da funcao
    n = length(x0); // Numero de variaveis do vetor de estados
    Dt = t(2)-t(1); // Passo de integracao (argumento da matriz de transicao)
    expansion_order = 10; //Numero de termos da expansao em serie de Taylor de exp(A*Dt)
    
    // Funcao para determinacao numerica da matriz de transicao
    function [f]=phi(Dt)
        f = eye(n,n); //Termo inicial da expansao de Taylor
        for k=1:expansion_order-1;
            f=f+(A^k)*(Dt^k)/factorial(k); // Somatorio da expansao
        end
    endfunction
    // Funcao para determinacao numerica da matriz dos termos forcantes
    function [g]=gama(Dt);
        g = Dt*eye(n,n); //Termo inicial da expansao de Taylor
        for k=1:expansion_order-1;
            g = g + (A^k)*(Dt^(k+1))/factorial(k+1); // Somatorio da expansao
        end
    endfunction
    
    // Laco de integracao
    y = zeros(n,length(t)); // Inicializacao da matriz cujas colunas  sao vetores de saida
    x(:,1)=x0; // Condicao inicial do vetor de estados do sistema de EDOs
    y(:,1) = C*x(:,1)+D*u(:,1); // Condicao inicial do vetor de saidas
    for i=2:length(t);
        x(:,i) = phi(Dt)*x(:,i-1)+gama(Dt)*B*u(:,i-1);
        y(:,i) = C*x(:,i)+D*u(:,i)
    end
    disp('Matriz de transicao:')
    disp(phi(Dt));
    disp('Matriz de termos forcantes:')
    disp(gama(Dt));
endfunction

z = integrador3(A,B,C,D,u,t,x0);
//plot2d(t,z(1,:)*180/%pi,2);