clear all;
f = 300e1;
m = 0.7;
L = 0.2;
th0 = 2;
th1 = 10.1;
g = 9.8;
t0 = 0;
tf = 10;
num = 500;
Jr=5.27e-5;
Jxx = 7e-3;
Jyy = 7e-3;
Jzz = 1.4e-2;
Kf = 2.57e-5;
Km = 5.6e-7;
alpha = 18.2e-3;
R = 1.2;
Ld = 0.02;
kt = 0.06;
w0 = 1.00*sqrt(m*g/(4*Kf))
V0 = w0*alpha;

t = linspace(t0,tf,num);


delta = 0.00001
function wr = w(t)
    if t > th0 then
        wr = [w0*(1-0.1*delta),w0*(1+0.1*delta),w0*(1+0.1*delta),w0*(1-0.1*delta)];
    else
        wr = [w0,w0,w0,w0];
    end
endfunction

function Ui = U(t)
    Ui(1) = Kf*(w(t)(1)^2 + w(t)(2)^2 + w(t)(3)^2 + w(t)(4)^2);
    Ui(2) = Kf*(-w(t)(2)^2 + w(t)(4)^2);
    Ui(3) = Kf*(w(t)(1)^2 - w(t)(3)^2);
    Ui(4) = Km*(w(t)(1)^2 - w(t)(2)^2 + w(t)(3)^2 - w(t)(4)^2);
endfunction

function tq = T(t)
    tq(1) = L*U(t)(3);
    tq(2) = L*U(t)(2);
    tq(3) = U(t)(4);
endfunction

x0 = 1;
xp0 = 0;
y0 = 1;
yp0 = 0;
z0 = 5;
zp0 = 0;
theta0 = 0.5*0.01745;
thetap0 = 0;
phi0 = 1* -0.01745;
phip0 = 0;
psi0 = 0;
psip0 = 0;

r0 = [x0;xp0;y0;yp0;z0;zp0;theta0;thetap0;phi0;phip0;psi0;psip0];


//Soluação nao linear
function dx=F(t,x)
    dx(1) = x(2);
    dx(2) = (U(t))(1)/m*(cos(x(9))*sin(x(7))*cos(x(11))+sin(x(9))*sin(x(11)));
    dx(3) = x(4);
    dx(4) =(U(t))(1)/m*(cos(x(9))*sin(x(7))*sin(x(11))-sin(x(9))*cos(x(11)));
    dx(5) = x(6);
    dx(6) = (U(t))(1)/m*(cos(x(9))*cos(x(7)))-g;
    dx(7) = x(8);
    dx(8) = (Jzz-Jxx)/Jyy*x(10)*x(12) - Jr/Jyy*x(10) + (T(t))(1)/Jyy;
    dx(9) = x(10);
    dx(10) = (Jyy-Jzz)/Jxx*x(8)*x(12) - Jr/Jxx*x(8) + (T(t)(2))/Jxx;
    dx(11) = x(12);
    dx(12) = (Jxx-Jyy)/Jzz*x(10)*x(8) + (T(t))(3)/Jzz;
endfunction

r = ode(r0,t0,t,F);




x = r(1,:); 
y = r(3,:); 
z = r(5,:); 
theta = r(7,:);
phi = r(9,:);
psi = r(11,:); 
xp = r(2,:); 
yp = r(4,:); 
zp = r(6,:); 
thetap = r(8,:); 
phip = r(10,:);
psip = r(12,:);



V1 = (1 - 1*delta)*V0;
V2 = (1 + 0*delta)*V0;
V3 = (1 + 1*delta)*V0;
V4 = (1 - 0*delta)*V0;

for i = 1:num
    if t(i) > th0 && t(i) < th1
        
        u(1,i) = V1^2 - V0^2;
        u(2,i) = V2^2 - V0^2;
        u(3,i) = V3^2 - V0^2;
        u(4,i) = V4^2 - V0^2;
    else
        u(1,i) = 0;
        u(2,i) = 0;
        u(3,i) = 0;
        u(4,i) = 0;
    end
end
r0L = [x0;xp0;y0;yp0;z0;zp0;phi0;phip0;theta0;thetap0;psi0;psip0];

A = zeros(12,12);
A(1,2) = 1;
A(2,9) = g;
A(3,4) = 1;
A(4,7) = -g;
A(5,6) = 1;
A(7,8) = 1;
A(9,10) = 1;
A(11,12) = 1;

B = zeros(12,4);
B2 = zeros(12,4);

B2(6,:) = Kf/m;
B2(8,2) = -L*Kf/Jxx; B2(8,4) = L*Kf/Jxx;
B2(10,1) = L*Kf/Jyy; B2(10,3) = -L*Kf/Jyy;
B2(12,1) = Km/Jzz;B2(12,2) = -Km/Jzz;B2(12,3) = Km/Jzz;B2(12,4) = -Km/Jzz;

B = (1/alpha^2)*B2


C = zeros(6,12);

C(1,1) = 1;
C(2,3) = 1;
C(3,5) = 1;
C(4,7) = 1;
C(5,9) = 1;
C(6,11) = 1;

D = zeros(6,4);

s = poly(0,"s");
sistema = syslin('c',A,B,C,D);
Y = csim(u,t,sistema,r0L);
G = C*inv(s*eye(12,12)-A)*B + D
disp(G)


x3 = Y(1,:)
y3 = Y(2,:)
z3 = Y(3,:)
phi3 = Y(4,:)
theta3 = Y(5,:)
psi3 = Y(6,:)



// Função que resolve o sistema no dominínio da frequência
function [x,y] = solverFrequencia(A,B,C,D,u,t,x0)

    function [mT,it] = matrizTransicao(A,ti)
        erro = 1e-10; Nmax = 15;
        n=length(x0)
        mT = eye(n,n);
            for i=1:Nmax
                mTNova = mT + (A^i)*(ti^i)/factorial(i);
                if mTNova - mT <= erro then break;
                else mT = mTNova; end
            end
            it = i;
    endfunction
        
    function [mR,it] = matrizResolvente(A,ti)
        erro = 1e-10; Nmax = 15;
        n=length(x0)
        mR = ti*eye(n,n);
            for i=1:Nmax
                mTNova = mR + (A^i)*(ti^(i+1))/factorial(i+1);
                if mTNova - mR <= erro then break;
                else mR = mTNova; end
            end
            it = i;
    endfunction

    // Loop no Tempo
    x(:,1) = x0;
    y (:,1) = C*x(:,1) + D*u(:,1);
    ti = t(2)-t(1);
    for i=2:length(t)
        x(:,i) = matrizTransicao(A,ti)*x(:,i-1) + matrizResolvente(A,ti)*B*u(:,i-1);
        y(:,i) = C * x(:,i) + D * u(:,i);
    end

endfunction


Y4 = solverFrequencia(A,B,C,D,u,t,r0L)

x4 = Y4(1,:)
y4 = Y4(3,:)
z4 = Y4(5,:)
phi4 = Y4(7,:)
theta4 = Y4(9,:)
psi4 = Y4(11,:)


comet3d(x3,y3,z3)

xtitle("Trajetória do drone (Modelo Linear)","X","Y","Z")



//f2 = scf(2);

//plot(t,[x;y;x3;y3])
//xtitle("Simulação do drone no plano XY","tempo em segundos","unidade (m)")
//legend("x","y","x linear", "y linear");
//a = gca();
//a.title.font_size = 3.5;
//a.x_label.font_size = 3.5;
//a.y_label.font_size = 3.5;
//a.font_size = 3.5;
//a.legend.font_size = 4;

//f3 = scf(3);
//subplot(111)
//plot(t,[z-z3])
//xtitle("$x_{linear}(t) - x_{Nlinear}(t)$","tempo em segundos","unidade (m)")
//legend(["x(t) não linear - x(t) linear"], pos=3);

//subplot(122)
//plot(t,[z;z3])
//xtitle("$x_{linear}(t) - x_{Nlinear}(t)$","tempo em segundos","unidade (m)")
//legend(["x(t) não linear";" x(t) linear"], pos=3);
//a = gca();
//a.title.font_size = 3.5;
//a.x_label.font_size = 3.5;
//a.y_label.font_size = 3.5;
//a.font_size = 3.5;
//a.legend.font_size = 4;