%% Programa para a definição dos parametros da simulacao

%   Este programa permite definir a posicao, velocidade e aceleracao
% angular de todas as juntas das pernas de um humanoide (2*6 DOF).
%   Este programa está  preparado para uma simulacao em SimMechanics
% na qual existe 6 modelos de Humanoide. A definicao da posicao, velocidade
% e aceleracao angular é feita a partir da definicao do tempo de ciclo para
% cada modelo realizar um dado movimento igual para todos os modelos.

close all
clear all
clc

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%% DETERMINACAO DOS PARAMETROS PARA OS 6 MODELOS %%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dados
TSim = [4 4 4 4 4 4]; %TempoSimulacao = 5s
NPS = 1000; %NumeroPontosSimulacao = 100pts
ETSim = TSim / NPS; %EncrmentoTempoSimulacao = TempoSimulacao / NumeroPontosSimulacao %
%% Calculo das Trajectorias desejadas para cada uma das juntas - Tt
% Este calculo é funcao de:
    %angulo inicial - T0
    %angulo final - Tf - Corresponde ao angulo máximo d
    %tempo de execucao do movimento - tf
        NJ = 0; %Numero da Junta
    for humanoide=1:1:6
        TS = TSim(1,humanoide);
        TSMax = max(TSim);
        ETS = ETSim(1,humanoide);
        %PernaDireita
            %Tornozelo
                %TornozeloDireitoX
                    T0 = 0; Tf = -20;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloDireitoX(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %TornozeloDireitoY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloDireitoY(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Joelho
                %JoelhoDireito
                    T0 = 0; Tf = 135;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoJoelhoDireito(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Anca
                %AncaDireitaX
                    T0 = 0; Tf = -135;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaX(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaDireitaY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaY(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaDireitaZ
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaZ(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
    
        %PernaEsquerda
            %Anca
                %AncaEsquerdaX
                    T0 = 0; Tf = -135;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaEsquerdaX(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaEsquerdaY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaEsquerdaY(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaEsquerdaZ
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaEsquerdaZ(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Joelho
                %JoelhoEsquerdo
                    T0 = 0; Tf = 135;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoJoelhoEsquerdo(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Tornozelo
                %TornozeloEsquerdoX
                    T0 = 0; Tf = -20;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloEsquerdoX(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %TornozeloEsquerdoY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloEsquerdoY(:,:,:,humanoide) = Movimento(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
    
        %Estrutura
            %Movimento Estrutura
                MovimentoEstrutura(:,:,:,humanoide) = [          ...
                MovimentoTornozeloDireitoX(:,:,:,humanoide); ...
                MovimentoTornozeloDireitoY(:,:,:,humanoide); ...
                MovimentoJoelhoDireito(:,:,:,humanoide);     ...
                MovimentoAncaDireitaX(:,:,:,humanoide);      ...
                MovimentoAncaDireitaY(:,:,:,humanoide);      ...
                MovimentoAncaDireitaZ(:,:,:,humanoide);      ...
                MovimentoAncaEsquerdaX(:,:,:,humanoide);     ...
                MovimentoAncaEsquerdaY(:,:,:,humanoide);     ...
                MovimentoAncaEsquerdaZ(:,:,:,humanoide);     ...
                MovimentoJoelhoEsquerdo(:,:,:,humanoide);    ...
                MovimentoTornozeloEsquerdoX(:,:,:,humanoide);...
                MovimentoTornozeloEsquerdoY(:,:,:,humanoide)];
        
    close all;
        
%% Preparacao dos dados para serem enviados para o Simulador
% Para o simulador é necessário enviar uma matriz de duas colunas
%em que na primeira temos o indice do tempo no qual queremos que o valor
%que se encontra na mesma linha e na segunda coluna seja enviado para o
%simulador

    ParametrosSimulacao(:,:,:,humanoide) = DadosSimulador(MovimentoEstrutura(:,:,:,humanoide),NPS,ETS);
    
    %Tornozelo Direito
    AnguloTornozeloDireitoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,1,humanoide);
      VelocidadeTornozeloDireitoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,2,humanoide);
        AceleracaoTornozeloDireitoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,3,humanoide);
    AnguloTornozeloDireitoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,4,humanoide);
      VelocidadeTornozeloDireitoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,5,humanoide);
        AceleracaoTornozeloDireitoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,6,humanoide);
        
    %Joelho Direito
    AnguloJoelhoDireito(:,:,:,humanoide) = ParametrosSimulacao(:,:,7,humanoide);
      VelocidadeJoelhoDireito(:,:,:,humanoide) = ParametrosSimulacao(:,:,8,humanoide);
        AceleracaoJoelhoDireito(:,:,:,humanoide) = ParametrosSimulacao(:,:,9,humanoide);
        
    %Anca Direita
    AnguloAncaDireitaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,10,humanoide);
      VelocidadeAncaDireitaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,11,humanoide);
        AceleracaoAncaDireitaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,12,humanoide);
    AnguloAncaDireitaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,13,humanoide);
      VelocidadeAncaDireitaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,14,humanoide);
        AceleracaoAncaDireitaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,15,humanoide);  
    AnguloAncaDireitaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,16,humanoide);
      VelocidadeAncaDireitaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,17,humanoide);
        AceleracaoAncaDireitaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,18,humanoide);
        
    %Anca Esquerda
    AnguloAncaEsquerdaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,19,humanoide);
      VelocidadeAncaEsquerdaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,20,humanoide);
        AceleracaoAncaEsquerdaX(:,:,:,humanoide) = ParametrosSimulacao(:,:,21,humanoide);
    AnguloAncaEsquerdaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,22,humanoide);
      VelocidadeAncaEsquerdaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,23,humanoide);
        AceleracaoAncaEsquerdaY(:,:,:,humanoide) = ParametrosSimulacao(:,:,24,humanoide);
    AnguloAncaEsquerdaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,25,humanoide);
      VelocidadeAncaEsquerdaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,26,humanoide);
        AceleracaoAncaEsquerdaZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,27,humanoide);
        
    %Joelho Esquerdo
    AnguloJoelhoEsquerdo(:,:,:,humanoide) = ParametrosSimulacao(:,:,28,humanoide);
      VelocidadeJoelhoEsquerdo(:,:,:,humanoide) = ParametrosSimulacao(:,:,29,humanoide);
        AceleracaoJoelhoEsquerdo(:,:,:,humanoide) = ParametrosSimulacao(:,:,30,humanoide);
        
    %Anca Esquerda
    AnguloTornozeloEsquerdoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,31,humanoide);
      VelocidadeTornozeloEsquerdoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,32,humanoide);
       AceleracaoTornozeloEsquerdoX(:,:,:,humanoide) = ParametrosSimulacao(:,:,33,humanoide);
    AnguloTornozeloEsquerdoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,34,humanoide);
      VelocidadeTornozeloEsquerdoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,35,humanoide);
       AceleracaoTornozeloEsquerdoY(:,:,:,humanoide) = ParametrosSimulacao(:,:,36,humanoide);
    end

%% Tempo de Espera ate fim da Simulacao

Tempo=0;
    while Tempo<max(TSim)
        Simulation='    Waiting for simulation'
        pause(0.5)
        clc
        Simulation='    Waiting for simulation .'
        pause(0.5)
        clc
        Simulation='    Waiting for simulation ..'
        pause(0.5)
        clc
        Simulation='    Waiting for simulation ...'
        pause(0.5)
        clc
    end
Simulation='    DONE :)'

%% Criacao dos Graficos para analise de resultados

figure(1)
Tempo=linspace(0,max(TSim),size(BinTorDirX1,1));


subplot(1,2,1)
hold on; grid on;
title('Binario Joelho para K diferentes (Bin VS Tmp)')
%line([0 max(Tempo)], [4 4],'COLOR',[1 0 0],'linewidth',2);
%line([0 max(Tempo)], [-4 -4],'COLOR',[1 0 0],'linewidth',2);
%plot(Tempo,BinJoeDir1,'COLOR',[1 0 0],'linewidth',2)
%plot(Tempo,BinJoeDir2,'COLOR',[0 0 1],'linewidth',2)
plot(Tempo,BinJoeDir3,'COLOR',[0 0.45 0],'linewidth',2)
%plot(Tempo,BinJoeDir4,'COLOR',[0 0.60 0],'linewidth',2)
%plot(Tempo,BinJoeDir5,'COLOR',[0 0.75 0],'linewidth',2)
%plot(Tempo,BinJoeDir6,'COLOR',[0 0.90 0],'linewidth',2)
subplot(1,2,2)
hold on; grid on;
title('Binario Joelho para K diferentes (Bin VS Ang)')
%line([min(AngJoeDir1) max(AngJoeDir1)], [4 4],'COLOR',[1 0 0],'linewidth',2);
%line([min(AngJoeDir1) max(AngJoeDir1)], [-4 -4],'COLOR',[1 0 0],'linewidth',2);
%plot(AngJoeDir1,BinJoeDir1,'COLOR',[1 0 0],'linewidth',1)
%plot(AngJoeDir1,BinJoeDir2,'COLOR',[0 0 1],'linewidth',1)
plot(AngJoeDir1,BinJoeDir3,'COLOR',[0 0.45 0],'linewidth',1)
%plot(AngJoeDir1,BinJoeDir4,'COLOR',[0 0.60 0],'linewidth',1)
%plot(AngJoeDir1,BinJoeDir5,'COLOR',[0 0.75 0],'linewidth',1)
%plot(AngJoeDir1,BinJoeDir6,'COLOR',[0 0.90 0],'linewidth',1)


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%