%%  DefinicaoParametrosSimulacao %%

close all
clear all
clc

%% Variaveis
TempoSimulacao = 1;             %TempoSimulacao = 5s
TempoFinal = TempoSimulacao / 2 
NumeroPontosSimulacao = 200;    %NumeroPontos = 1000
PontosSimulacao = linspace(0,TempoSimulacao,NumeroPontosSimulacao);
EncrementoSimulacao = TempoSimulacao/NumeroPontosSimulacao;

%% Tornozelo Perna Direita
%   AnguloTornozeloX
figure(1); grid on; hold on;
AnguloTornozeloX = -20
for t=1:1:size(PontosSimulacao,2)/2
    AnguloTornozeloX1 = (-2/TempoFinal^3)*(AnguloTornozeloX)*PontosSimulacao(1,t)^3 + (3/TempoFinal^2)*(AnguloTornozeloX)*PontosSimulacao(1,t)^2;      %Polinomio 3ºgrau para 1ªmetade do movimento
    AnguloTornozeloX3(t,1) = AnguloTornozeloX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AnguloTornozeloX2 = -(-2/TempoFinal^3)*(AnguloTornozeloX)*(PontosSimulacao(1,t)-TempoFinal)^3 - (3/TempoFinal^2)*(AnguloTornozeloX)*(PontosSimulacao(1,t)-TempoFinal)^2 - 20;
    AnguloTornozeloX3(t,1) = AnguloTornozeloX2;
end

for t = 1:1:size(AnguloTornozeloX3,1)
    plot(AnguloTornozeloX3,'og')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AnguloTornozeloX(t,1) = Tempo;
    AnguloTornozeloX(t,2) = AnguloTornozeloX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%   VelocidadeTornozeloX
figure(1)

for t=1:1:size(PontosSimulacao,2)/2
    VelocidadeTornozeloX1 = 3*(-2/TempoFinal^3)*(AnguloTornozeloX)*PontosSimulacao(1,t)^2 + 2*(3/TempoFinal^2)*(AnguloTornozeloX)*PontosSimulacao(1,t);      %Polinomio 3ºgrau para 1ªmetade do movimento
    VelocidadeTornozeloX3(t,1) = VelocidadeTornozeloX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    VelocidadeTornozeloX2 = -3*(-2/TempoFinal^3)*(AnguloTornozeloX)*(PontosSimulacao(1,t)-TempoFinal)^2 - 2*(3/TempoFinal^2)*(AnguloTornozeloX)*(PontosSimulacao(1,t)-TempoFinal);
    VelocidadeTornozeloX3(t,1) = VelocidadeTornozeloX2;
end

for t = 1:1:size(VelocidadeTornozeloX3,1)
    plot(VelocidadeTornozeloX3,'ob')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    VelocidadeTornozeloX(t,1) = Tempo;
    VelocidadeTornozeloX(t,2) = VelocidadeTornozeloX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%	AceleracaoTornozeloX
figure(1)

for t=1:1:size(PontosSimulacao,2)/2
    AceleracaoTornozeloX1 = 2*3*(-2/TempoFinal^3)*(AnguloTornozeloX)*PontosSimulacao(1,t) + 2*1*(3/TempoFinal^2)*(AnguloTornozeloX);      %Polinomio 3ºgrau para 1ªmetade do movimento
    AceleracaoTornozeloX3(t,1) = AceleracaoTornozeloX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AceleracaoTornozeloX2 = -3*2*(-2/TempoFinal^3)*(AnguloTornozeloX)*(PontosSimulacao(1,t)-TempoFinal) - 2*1*(3/TempoFinal^2)*(AnguloTornozeloX);
    AceleracaoTornozeloX3(t,1) = AceleracaoTornozeloX2;
end

for t = 1:1:size(VelocidadeTornozeloX3,1)
    plot(AceleracaoTornozeloX3,'or')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AceleracaoTornozeloX(t,1) = Tempo;
    AceleracaoTornozeloX(t,2) = AceleracaoTornozeloX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%% Joelho Perna Direita
%   AnguloJoelho
figure(2); grid on; hold on;
AnguloJoelho = 135

for t=1:1:size(PontosSimulacao,2)/2
    AnguloJoelho1 = (-2/TempoFinal^3)*(AnguloJoelho)*PontosSimulacao(1,t)^3 + (3/TempoFinal^2)*(AnguloJoelho)*PontosSimulacao(1,t)^2      %Polinomio 3ºgrau para 1ªmetade do movimento
    AnguloJoelho3(t,1) = AnguloJoelho1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AnguloJoelho2 = -(-2/TempoFinal^3)*(AnguloJoelho)*(PontosSimulacao(1,t)-TempoFinal)^3 - (3/TempoFinal^2)*(AnguloJoelho)*(PontosSimulacao(1,t)-TempoFinal)^2 + AnguloJoelho
    AnguloJoelho3(t,1) = AnguloJoelho2;
end

for t = 1:1:size(AnguloJoelho3,1)
    plot(AnguloJoelho3,'og')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AnguloJoelho(t,1) = Tempo;
    AnguloJoelho(t,2) = AnguloJoelho3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%	VelocidadeJoelho
figure(2)

for t=1:1:size(PontosSimulacao,2)/2
    VelocidadeJoelho1 = 3*(-2/TempoFinal^3)*(AnguloJoelho)*PontosSimulacao(1,t)^2 + 2*(3/TempoFinal^2)*(AnguloJoelho)*PontosSimulacao(1,t)      %Polinomio 3ºgrau para 1ªmetade do movimento
    VelocidadeJoelho3(t,1) = VelocidadeJoelho1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    VelocidadeJoelho2 = -3*(-2/TempoFinal^3)*(AnguloJoelho)*(PontosSimulacao(1,t)-TempoFinal)^2 - 2*(3/TempoFinal^2)*(AnguloJoelho)*(PontosSimulacao(1,t)-TempoFinal)
    VelocidadeJoelho3(t,1) = VelocidadeJoelho2;
end

for t = 1:1:size(VelocidadeJoelho3,1)
    plot(VelocidadeJoelho3,'ob')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    VelocidadeJoelho(t,1) = Tempo;
    VelocidadeJoelho(t,2) = VelocidadeJoelho3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%	AceleracaoJoelho
figure(2)

for t=1:1:size(PontosSimulacao,2)/2
    AceleracaoJoelho1 = 3*2*(-2/TempoFinal^3)*(AnguloJoelho)*PontosSimulacao(1,t) + 2*1*(3/TempoFinal^2)*(AnguloJoelho)      %Polinomio 3ºgrau para 1ªmetade do movimento
    AceleracaoJoelho3(t,1) = AceleracaoJoelho1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AceleracaoJoelho2 = -3*2*(-2/TempoFinal^3)*(AnguloJoelho)*(PontosSimulacao(1,t)-TempoFinal) - 2*1*(3/TempoFinal^2)*(AnguloJoelho)
    AceleracaoJoelho3(t,1) = AceleracaoJoelho2;
end

for t = 1:1:size(VelocidadeJoelho3,1)
    plot(AceleracaoJoelho3,'or')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AceleracaoJoelho(t,1) = Tempo;
    AceleracaoJoelho(t,2) = AceleracaoJoelho3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%% Anca Perna Direita
%   AnguloAncaX
figure(3); grid on; hold on;
AnguloAncaX

for t=1:1:size(PontosSimulacao,2)/2
    AnguloAncaX1 = (-2/TempoFinal^3)*(AnguloAncaX)*PontosSimulacao(1,t)^3 + (3/TempoFinal^2)*(AnguloAncaX)*PontosSimulacao(1,t)^2      %Polinomio 3ºgrau para 1ªmetade do movimento
    AnguloAncaX3(t,1) = AnguloAncaX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AnguloAncaX2 = -(-2/TempoFinal^3)*(AnguloAncaX)*(PontosSimulacao(1,t)-TempoFinal)^3 - (3/TempoFinal^2)*(AnguloAncaX)*(PontosSimulacao(1,t)-TempoFinal)^2 + AnguloAncaX
    AnguloAncaX3(t,1) = AnguloAncaX2;
end

for t = 1:1:size(AnguloAncaX3,1)
    plot(AnguloAncaX3,'og')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AnguloAncaX(t,1) = Tempo;
    AnguloAncaX(t,2) = AnguloAncaX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%	VelocidadeAncaX
figure(3)

for t=1:1:size(PontosSimulacao,2)/2
    VelocidadeAncaX1 = 3*(-2/TempoFinal^3)*(AnguloAncaX)*PontosSimulacao(1,t)^2 + 2*(3/TempoFinal^2)*(AnguloAncaX)*PontosSimulacao(1,t)      %Polinomio 3ºgrau para 1ªmetade do movimento
    VelocidadeAncaX3(t,1) = VelocidadeAncaX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    VelocidadeAncaX2 = -3*(-2/TempoFinal^3)*(AnguloAncaX)*(PontosSimulacao(1,t)-TempoFinal)^2 - 2*(3/TempoFinal^2)*(AnguloAncaX)*(PontosSimulacao(1,t)-TempoFinal)
    VelocidadeAncaX3(t,1) = VelocidadeAncaX2;
end

for t = 1:1:size(VelocidadeAncaX3,1)
    plot(VelocidadeAncaX3,'ob')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    VelocidadeAncaX(t,1) = Tempo;
    VelocidadeAncaX(t,2) = VelocidadeAncaX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

%	AceleracaoAncaX
figure(3)

for t=1:1:size(PontosSimulacao,2)/2
    AceleracaoAncaX1 = 3*2*(-2/TempoFinal^3)*(AnguloAncaX)*PontosSimulacao(1,t) + 2*1*(3/TempoFinal^2)*(AnguloAncaX);      %Polinomio 3ºgrau para 1ªmetade do movimento
    AceleracaoAncaX3(t,1) = AceleracaoAncaX1;
end

for t=size(PontosSimulacao,2)/2:1:size(PontosSimulacao,2)
    AceleracaoAncaX2 = -3*2*(-2/TempoFinal^3)*(AnguloAncaX)*(PontosSimulacao(1,t)-TempoFinal) - 2*1*(3/TempoFinal^2)*(AnguloAncaX)
    AceleracaoAncaX3(t,1) = AceleracaoAncaX2;
end

for t = 1:1:size(VelocidadeAncaX3,1)
    plot(AceleracaoAncaX3,'or')
end

Tempo = EncrementoSimulacao; 
for t=1:1:NumeroPontosSimulacao
    AceleracaoAncaX(t,1) = Tempo;
    AceleracaoAncaX(t,2) = AceleracaoAncaX3(t,1);
    Tempo=Tempo+EncrementoSimulacao;
end

pause(10)
%close all