// Trabalho de PME3380 - Modelagem de Sistemas Dinâmicos

clear()
//Parâmetros Iniciais
m1 = 0.44
m2 = 0.3
m3 = 0.18
m4 = 0.765
p = 0.03
c = 0.01
g = 9.8
x0 = 0
xponto0 = 0
y0 = 0
yponto0 = 0
z0 = 0
zponto0 = 0
passos = 0
total_passos = 30
pas = 1000 + 200*total_passos
t1 = zeros(1:pas)
t2 = zeros(1:pas)
t3 = zeros(1:pas)
t4 = zeros(1:pas)
x = zeros(1:pas)
xponto = zeros(1:pas)
y = zeros(1:pas)
yponto = zeros(1:pas)
t = linspace(0,1,pas)//para 10s de simulação
i = int(1);
txi = 0
tyi = 0

while passos <= total_passos
    i = 1
    while i <= pas 
        if 0 + 200*(passos) < i && i <= 200*(1 + passos) then
            t1(i) = 0.00;
            t2(i) = 0.01*g;
            t3(i) = 0.01*g;
            t4(i) = 0.00;
           
        elseif  (200*(1 + passos)< i) && (i <= 400 + 200*passos) then   
            t1(i) = 0.04*g;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.00; 
           
        elseif  (400 + 200*passos < i) && (i <= 600 + 200*passos) then
            t1(i) = 0.00;
            t2(i) = -0.01*g;
            t3(i) = -0.01*g;
            t4(i) = 0.00; 
           
        elseif (600 + 200*passos < i) && (i <= 800 + 200*passos) then
            t1(i) = -0.04*g;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.00;
       
        elseif (820 + 200*passos < i) && (i <= 1000 + 200*passos) then
            t1(i) = 0.00;
            t2(i) = 0.00;
            t3(i) = 0.00;
            t4(i) = 0.01*g;     
        end
        i = i + 1
    end
    passos = passos + 5
end
    
funcprot(0)
function dkx = cabecotex(t,kx)   
        dkx(1) = kx(2)
        dkx(2) = ((2*%pi*t1(i)/p)-(2*c*(kx(2))))*1/(m1+m3)
endfunction

function dky = cabecotey(t,ky)
        dky(1) = ky(2)
        dky(2) = ((2*%pi*(t2(i)+t3(i))/p) - 2*c*(ky(2)))*(1/m1+m2+3*m3)
endfunction

function dkz = cabecotez(t,kz)
        dkz(1) = kz(2)
        dkz(2) = ((2*%pi*((t4(i))/p))+m4*g - c*(kz(2)))*(1/(m4+m3/2))
endfunction

i = 1
while i <= pas     
    j = 1
    while j <= 200 
        if i == 1 then   
            y(i) = 0
            yponto(i) = 0            
            x(i) = 0
            xponto(i) = 0       
        elseif t1(i) == 0 && t2(i) ~= 0 && t3(i) ~= 0 then  
            if tyi == 0 then             
                ky = ode([y0;yponto0],0,t(j),cabecotey)    
                    y(i) = ky(1,size(ky,2));
                    yponto(i) = ky(2,size(ky,2));
            elseif tyi ~= 0 then
                ky = ode([y0;yponto0],0,t(j),cabecotey)    
                    y(i) = y(tyi) + ky(1,size(ky,2));
                    yponto(i) = ky(2,size(ky,2));
            end   
            x(i) = x(i-1)               
            if modulo(i,200) == 0 && j == 200 then
                tyi = i - 1    
            end                   
        elseif t2(i) == 0 && t3(i) == 0 && t1(i) ~= 0 then 
            if txi == 0 then        
                kx = ode([y0;yponto0],0,t(j),cabecotex)
                    x(i) = kx(1,size(kx,2));
                    xponto(i) = kx(2,size(kx,2));
            elseif txi ~= 0 then
                kx = ode([x0;xponto0],0,t(j),cabecotex)
                    x(i) = x(txi) + kx(1,size(kx,2));
                    xponto(i) = kx(2,size(kx,2));
            end                
            y(i) = y(i-1)        
            if modulo(i,200) == 0 && j == 200 then
                txi = i - 1   
            end            
        
        elseif t1(i) == 0 && t2(i) == 0 && t3(i) == 0 then
            y(i) = y(i-1)
            x(i) = x(i-1)       
        end
        j = j + 1                      
    i = i + 1
    end;               
end

zponto = zeros(1:pas)
z= zeros(1:pas)

i = 1
tzi = 0

while i <= pas     
    j = 1
    while j <= 20 
        if i == 1 then   
            z(i) = 0
            zponto(i) = 0             
        elseif t4(i) ~= 0 then  
            if tzi == 0 then             
                kz = ode([z0;zponto0],0,t(j),cabecotez)    
                    z(i) = kz(1,size(kz,2));
                    zponto(i) = kz(2,size(kz,2));
            elseif tzi ~= 0 then
                kz = ode([z0;zponto0],0,t(j),cabecotez)    
                    z(i) = z(tzi) + kz(1,size(kz,2));
                    zponto(i) =  kz(2,size(kz,2));
            end   
                         
            if modulo(i,20) == 0 && j == 20 then       
                tzi = i - 1    
            end      
        elseif t4(i) == 0 then
            z(i) = z(i-1)
        end
        j = j + 1                             
    i = i + 1
    end; 
   
end            



//definindo omegas
r = sqrt(x^2+y^2)
omega1 = xponto/r
omega2 = yponto/r
omega3 = zponto/r

//Energias

    T = 0.5*m2*(yponto^2)+m3*yponto^2+0.5*m1*yponto^2+0.5*m3*r^2*omega1^2+0.5*m3*r^2*omega2^2+0.5*m3*r^2*omega3^2 //Energia Cinética
    V = m4*g*z //Energia Potencial
    Em = V + T //energia mecânica



da=sda() // obtendo manipulador dos eixo modelos para ver e editar campos
// título padrão
da.title.text="My Default@Title"
da.title.font_size = 5;
// rótulos x padrões
da.x_label.font_size = 3;
// rótulos y padrões
da.y_label.font_style = 3;
da.y_label.font_size = 3;
da.y_location = "left";

//plotando gráficos
f1 = scf(1)
plot(t,x,"b");
xtitle("X em função do tempo", "Tempo","X")

f2 = scf(2)
plot(t,y,"b");
xtitle("Y em função do tempo", "Tempo","Y")

f3 = scf(3)
plot(t,xponto,"b");
xtitle("Xponto em função do tempo", "Tempo","Xponto")

f4 = scf(4)
plot(t,z,"b");
xtitle("z em função do tempo", "Tempo","z")

f5 = scf(5)
plot(t,zponto,"b");
xtitle("zponto em função do tempo", "Tempo","z")

f6 = scf(6)
plot(t,yponto,"b");
xtitle("Yponto em função do tempo", "Tempo","Yponto")

f7 = scf(7)
plot2d(x,y)
xtitle("Posição do cabeçote", "Eixo X","Eixo Y")

f8 = scf(8)
comet3d(x,y,z)
xtitle("")