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

%   Este programa permite definir a posicao, velocidade e aceleracao
% angular de todas as juntas de uma perna de um humanoide (6 DOF).
%   Este programa está  preparado para uma simulacao em SimMechanics
% na qual existe 1 modelo de uma perna de 1 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 O MODELO %%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Dados
TSim = [5]; %TempoSimulacao = 5s
NPS = 1000; %NumeroPontosSimulacao = 1000pts
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
    %tempo de execucao do movimento - tf
        NJ = 0; %Numero da Junta
    for humanoide=1:1:1
        TS = TSim(1,humanoide);
        TSMax = max(TSim);
        ETS = ETSim(1,humanoide);
        %PernaDireita
            %Tornozelo
                %TornozeloDireitoX
                    T0 = 0; Tf = -55;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloDireitoX(:,:,:,humanoide) = Movimento5grau(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %TornozeloDireitoY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoTornozeloDireitoY(:,:,:,humanoide) = Movimento5grau(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Joelho
                %JoelhoDireito
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoJoelhoDireito(:,:,:,humanoide) = Movimento5grau(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
            %Anca
                %AncaDireitaX
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaX(:,:,:,humanoide) = Movimento5grau(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaDireitaY
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaY(:,:,:,humanoide) = Movimento5grau(T0,Tf,tf,TS,NPS,ETS,NJ,TSMax);
                %AncaDireitaZ
                    T0 = 0; Tf = 0;
                    tf = TS/2;
                    NJ = NJ+1;
                    MovimentoAncaDireitaZ(:,:,:,humanoide) = Movimento5grau(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)];
    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
    AngTorDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,1,humanoide);
      VelTorDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,2,humanoide);
        AceTorDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,3,humanoide);
    AngTorDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,4,humanoide);
      VelTorDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,5,humanoide);
        AceTorDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,6,humanoide);
        
    %Joelho Direito
    AngJoeDir(:,:,:,humanoide) = ParametrosSimulacao(:,:,7,humanoide);
      VelJoeDir(:,:,:,humanoide) = ParametrosSimulacao(:,:,8,humanoide);
        AceJoeDir(:,:,:,humanoide) = ParametrosSimulacao(:,:,9,humanoide);
        
    %Anca Direita
    AngAncDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,10,humanoide);
      VelAncDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,11,humanoide);
        AceAncDirX(:,:,:,humanoide) = ParametrosSimulacao(:,:,12,humanoide);
    AngAncDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,13,humanoide);
      VelAncDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,14,humanoide);
        AceAncDirY(:,:,:,humanoide) = ParametrosSimulacao(:,:,15,humanoide);  
    AngAncDirZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,16,humanoide);
      VelAncDirZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,17,humanoide);
        AceAncDirZ(:,:,:,humanoide) = ParametrosSimulacao(:,:,18,humanoide);
        
    end
%% Definição dos valores de k, b e r0 dos conjuntos mola amortecedor do simulador
    
    MolTorDir1 = 1000;
    MolTorDir2 = 1000;
    MolTorDir3 = 1000;
    MolTorDir4 = 1000;
    
    
    
%% Tempo de Espera ate fim da Simulacao
Tempo=0;
    while max(Tempo)<max(TSim)-0.000001
        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(MedBinTorDirX,1));

%% COISO
figure(2)
axis([min(Tempo) max(Tempo) 1.2*min(MedAngTorDirX) 1.2*max(MedAngTorDirX)])
subplot(1,3,1)
hold on; grid on;
title('Angulo')
plot(Tempo,MedAngTorDirX,'COLOR',[0 0.45 0],'linewidth',2)
line([max(Tempo)/2 max(Tempo)/2],[1.1*min(MedAngTorDirX) 1.1*max(MedAngTorDirX)],'COLOR',[1 0 0],'linewidth',1)
subplot(1,3,2)
hold on; grid on;
title('Velocidade')
plot(Tempo,MedVelTorDirX,'COLOR',[0 0.45 0],'linewidth',2)
line([max(Tempo)/2 max(Tempo)/2],[1.1*min(MedVelTorDirX) 1.1*max(MedVelTorDirX)],'COLOR',[1 0 0],'linewidth',1)
subplot(1,3,3)
hold on; grid on;
title('Aceleracao')
plot(Tempo,MedAceTorDirX,'COLOR',[0 0.45 0],'linewidth',2)
line([max(Tempo)/2 max(Tempo)/2],[1.1*min(MedAceTorDirX) 1.1*max(MedAceTorDirX)],'COLOR',[1 0 0],'linewidth',1)

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

figure(1)
%% TORNOZELO X
subplot(2,1,1)
hold on; grid off;
%axis([min(Tempo) max(Tempo) 1.1*min(MedBinTorDirX) 0*max(MedBinTorDirX)])
title('Binario Tornozelo para K diferentes (Bin VS Tmp)')
plot(Tempo,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
    MediaBinTorDirX = sum(MedBinTorDirX)/size(MedBinTorDirX,1);
    ValMaxBinTorDirX = max(abs(MedBinTorDirX))
        line( [0 max(Tempo)], [MediaBinTorDirX MediaBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
        subplot(2,1,2)
        line( [0 max(AngTorDirX(:,2))], [MediaBinTorDirX MediaBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
        subplot(2,1,1)
        if max(MedBinTorDirX) < max(abs(MedBinTorDirX))
            line( [0 max(Tempo)], [-ValMaxBinTorDirX -ValMaxBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
            subplot(2,1,2)
            line( [0 max(AngTorDirX(:,2))], [-ValMaxBinTorDirX -ValMaxBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
        else
            line( [0 max(Tempo)], [ValMaxBinTorDirX ValMaxBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
            subplot(2,1,2)
            line( [0 max(AngTorDirX(:,2))], [ValMaxBinTorDirX ValMaxBinTorDirX],'COLOR',[0 0 0],'linewidth',1 )
        end
        subplot(2,1,1)
        line([max(Tempo)/2 max(Tempo)/2],[min(MedBinTorDirX) max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
plot(Tempo,MedBinTorDirX1,'COLOR',[0 1 0],'linewidth',2)
    MediaBinTorDirX1 = sum(MedBinTorDirX1)/size(MedBinTorDirX1,1);
    ValMaxBinTorDirX1 = max(abs(MedBinTorDirX1))
        line( [0 max(Tempo)], [MediaBinTorDirX1 MediaBinTorDirX1],'COLOR',[0 1 0],'linewidth',1 )
        subplot(2,1,2)
        line( [0 max(AngTorDirX(:,2))], [MediaBinTorDirX1 MediaBinTorDirX1],'COLOR',[0 1 0],'linewidth',1 )
        subplot(2,1,1)
        if max(MedBinTorDirX1) < max(abs(MedBinTorDirX1))
            line( [0 max(Tempo)], [-ValMaxBinTorDirX1 -ValMaxBinTorDirX1],'COLOR',[0 1 0],'linewidth',1 )
            subplot(2,1,2)
            line( [0 max(AngTorDirX(:,2))], [-ValMaxBinTorDirX1 -ValMaxBinTorDirX1],'COLOR',[0 1 0],'linewidth',1 )
        else
            line( [0 max(Tempo)], [ValMaxBinTorDirX1 ValMaxBinTorDirX1],'COLOR',[0 1 0],'linewidth',1);
            subplot(2,1,2)
            line( [0 max(AngTorDirX(:,2))], [ValMaxBinTorDirX1 ValMaxBinTorDirX1],'COLOR',[0 1 0],'linewidth',1);
        end
        subplot(2,1,1)
%% TORNOZELO Y
%subplot(2,2,2)
%hold on; grid on;
%title('Binario Tornozelo para K diferentes (Bin VS Tmp)')
%plot(Tempo,MedBinTorDirY,'COLOR',[0 0 0],'linewidth',2)
%    MediaBinTorDirY = sum(MedBinTorDirY)/size(MedBinTorDirY,1);
%    ValMaxBinTorDirY = max(abs(MedBinTorDirY));
%        plot(Tempo,MediaBinTorDirY,'COLOR',[0 0 0],'linewidth',2);
%        if max(MedBinTorDirY) < max(abs(MedBinTorDirY))
%            plot(Tempo,-ValMaxBinTorDirY,'COLOR',[0 0 0],'linewidth',2);
%        else
%            plot(Tempo,ValMaxBinTorDirY,'COLOR',[0 0 0],'linewidth',2);
%        end
%plot(Tempo,MedBinTorDirY1,'COLOR',[0 1 0],'linewidth',2)
%    MediaBinTorDirY1 = sum(MedBinTorDirY1)/size(MedBinTorDirY1,1);
%    ValMaxBinTorDirY1 = max(abs(MedBinTorDirY1));
%        plot(Tempo,MediaBinTorDirY1,'COLOR',[0 1 0],'linewidth',2);
%        if max(MedBinTorDirY1) < max(abs(MedBinTorDirY1))
%            plot(Tempo,-ValMaxBinTorDirY1,'COLOR',[0 1 0],'linewidth',2);
%        else
%            plot(Tempo,ValMaxBinTorDirY1,'COLOR',[0 1 0],'linewidth',2);
%        end

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

subplot(2,1,2)
hold on; grid off;
%axis([min(AngTorDirX(:,2)) 1.0*max(AngTorDirX(:,2)) 1.1*min(MedBinTorDirX) 0.0*max(MedBinTorDirX)])
title('Binario Tornozelo X para K diferentes (Bin VS Ang)')
plot(MedAngTorDirX,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
line([max(AngTorDirX(:,2)) max(AngTorDirX(:,2))],[min(MedBinTorDirX) max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
plot(MedAngTorDirX,MedBinTorDirX1,'COLOR',[0 1 0],'linewidth',2)


%subplot(2,2,4)
%hold on; grid on;
%title('Binario Tornozelo Y para K diferentes (Bin VS Ang)')
%plot(MedAngTorDirY,MedBinTorDirY,'COLOR',[0 0 0],'linewidth',1)
%plot(MedAngTorDirY,MedBinTorDirY1,'COLOR',[0 1 0],'linewidth',1)

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