%% 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) assim
% como os valores de k, b e r0 de cada actuador passivo.
%   Este programa está  preparado para uma simulacao em SimMechanics
% na qual existe 2 modelo de 1 perna de 1 Humanoide.

close all
clear all
clc

%% CICLO for DE TODO O PROGRAMA
% permite correr varias simulações com incrementos do valor de k.

    ki = 0;                             % Valor inicial do k
    kf = 200;                           % Valor final do k
    ik = 1/5 * (kf - ki);               % Incremento do valor do k é 1/5 do intervalo
    NumeroSimulacoes = (kf-ki)/ik+1;    % Numero de simulações que vão ser realizadas 5+1
    bi = 0;                             % Constante de Amortecimento
    r0 = 0.010;                         % Comprimento inicial do elemento (m)
    ikmin = 1;                          % Condição de paragem, valor minimo do incremento de k

while ik > ikmin                        % Condição de permanencia no ciclo que permite determinar o k optimizado
    
    for simulacao = 1 : 1 : NumeroSimulacoes    % Condição de permanencia no ciclo que permite fazer os varios ensaios para os varios k do intervalo em estudo

        %% Dados
            TSim = [5];         % TempoSimulacao = 5s
            NPS  = 2000;        % NumeroPontosSimulacao = 2000pts
            ITSim = TSim / NPS; % IncrmentoTempoSimulacao = TempoSimulacao / NumeroPontosSimulacao
        %% Calculo das Trajectorias desejadas para cada uma das juntas - Tt
            % Este calculo é funcao de:
                % angulo inicial - T0 - Corresponde ao angulo inicial (IC)
                % angulo final   - Tf - Corresponde ao angulo máximo
                % tempo de execucao do movimento - tf
            NJ = 0; %Numero da Junta
            for humanoide=1:1:1     % permite fazer a distincção entre os varios modelos do simulador
                TS = TSim(1,humanoide);     %Cada modelo do simulador pode ter um tempo de simulação diferente
                TSMax = max(TSim);          %Tempo total da simulação igual a simulação mais lenta
                ITS = ITSim(1,humanoide);   %Incremento de tempo de cada um dos varios modelos do simulador
                %% PernaDireita
                    %% Tornozelo
                        %TornozeloDireitoX
                            T0 = 0; Tf = 60;    %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoTornozeloDireitoX(:,:,:,humanoide) = ...   %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
                        %TornozeloDireitoY
                            T0 = 0; Tf = 0;     %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoTornozeloDireitoY(:,:,:,humanoide) = ...   %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
                    %% Joelho
                        %JoelhoDireito
                            T0 = 0; Tf = 0;     %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoJoelhoDireito(:,:,:,humanoide) = ...       %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
                    %% Anca
                        %AncaDireitaX
                            T0 = 0; Tf = 0;     %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoAncaDireitaX(:,:,:,humanoide) = ...        %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
                        %AncaDireitaY
                            T0 = 0; Tf = 0;     %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoAncaDireitaY(:,:,:,humanoide) = ...        %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
                        %AncaDireitaZ
                            T0 = 0; Tf = 0;     %|
                            tf = TS/2;          %|DADOS
                            NJ = NJ+1;          %|
                            MovimentoAncaDireitaZ(:,:,:,humanoide) = ...        %|
                                Movimento5grau(T0,Tf,tf,TS,NPS,ITS,NJ,TSMax);   %|TRAJECTORIA
    
                %%PernaDireita
                    %% Tornozelo
                    %% Joelho
                    %% Anca

                %% Estrutura
                    %Movimento Estrutura (neste caso so temos a perna direita)
                        MovimentoEstrutura(:,:,:,humanoide) = [          ...
                            MovimentoTornozeloDireitoX(:,:,:,humanoide); ...
                            MovimentoTornozeloDireitoY(:,:,:,humanoide); ...
                            MovimentoJoelhoDireito(:,:,:,humanoide);     ...
                            MovimentoAncaDireitaX(:,:,:,humanoide);      ...
                            MovimentoAncaDireitaY(:,:,:,humanoide);      ...
                            MovimentoAncaDireitaZ(:,:,:,humanoide)];

        %% 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,ITS);
    
            %% 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
            
            if simulacao==1
                MolRigTorDir1 = ki;  MolAmoTorDir1 = bi;  MolDimTorDir1 = r0;    % [N/m]  []  [m]
                MolRigTorDir2 = ki;  MolAmoTorDir2 = bi;  MolDimTorDir2 = r0;    % [N/m]  []  [m]
                MolRigTorDir3 = ki;  MolAmoTorDir3 = bi;  MolDimTorDir3 = r0;    % [N/m]  []  [m]
                MolRigTorDir4 = ki;  MolAmoTorDir4 = bi;  MolDimTorDir4 = r0;    % [N/m]  []  [m]
            else
                MolRigTorDir1 = MolRigTorDir1+ik;  MolAmoTorDir1 = bi;  MolDimTorDir1 = r0;    % [N/m]  []  [m]
                MolRigTorDir2 = MolRigTorDir2+ik;  MolAmoTorDir2 = bi;  MolDimTorDir2 = r0;    % [N/m]  []  [m]
                MolRigTorDir3 = MolRigTorDir3+ik;  MolAmoTorDir3 = bi;  MolDimTorDir3 = r0;    % [N/m]  []  [m]
                MolRigTorDir4 = MolRigTorDir4+ik;  MolAmoTorDir4 = bi;  MolDimTorDir4 = r0;    % [N/m]  []  [m]
            end

        %% Tempo de Espera ate fim da Simulacao
            Tempo=0;
            startsimulation=1;
            if simulacao == NumeroSimulacoes+1
                break
            else
                while max(Tempo) < max(TSim)-0.000001
                    if startsimulation == 1
                        sim('SoTornozelo')
                        startsimulation = 0;
                    end
                    DadosSimulacao = [simulacao MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4]
                    Simulation='    Waiting for simulation'
                    %pause(0.1)
                    clc
                    DadosSimulacao = [simulacao MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4]
                    Simulation='    Waiting for simulation .'
                    %pause(0.1)
                    clc
                    DadosSimulacao = [simulacao MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4]
                    Simulation='    Waiting for simulation ..'
                    %pause(0.1)
                    clc
                    DadosSimulacao = [simulacao MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4]
                    Simulation='    Waiting for simulation ...'
                    %pause(0.1)
                    clc
                end
            end
            Simulation = '    DONE :)'

        %% Criação dos graficos relativos a trajectoria imposta
%            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)

        %% Criacao dos Graficos para analise de resultados

%            figure(1)
            Tempo=linspace(0,max(TSim),size(MedBinTorDirX,1));
            
            %% TORNOZELO X
%                subplot(2,1,1);    hold on;    grid off;    title('Binario Tornozelo para K diferentes (Bin VS Tmp)')
                %axis([min(Tempo) max(Tempo) 1.1*min(MedBinTorDirX) 0*max(MedBinTorDirX)])
%                plot(Tempo,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
                MediaBinTorDirX = sum(MedBinTorDirX)/size(MedBinTorDirX,1);
                MediaQuadraticaBinTorDirX = sum(MedBinTorDirX.^2)/size(MedBinTorDirX,1);
                Medias(simulacao , :) = [MediaBinTorDirX MediaQuadraticaBinTorDirX MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4];
                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);
                MediaQuadraticaBinTorDirX1 = sum(MedBinTorDirX1.^2)/size(MedBinTorDirX1,1);
                Medias1(simulacao , :) = [MediaBinTorDirX1 MediaQuadraticaBinTorDirX1 MolRigTorDir1 MolRigTorDir2 MolRigTorDir3 MolRigTorDir4];
                    MedBin(simulacao , :) = MedBinTorDirX1;
                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)
                
%                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)
                
            %% 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,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)
    end
    
    for n=1:1:NumeroSimulacoes-1
        if min(Medias1(:,2)) == Medias1(n,2)    %%Criterio de seleccao min(MediaQuadraticaBinTorDirX1)
            MediaQuadraticaMinima = Medias1(n,:)
            MelhorSimulacao = n;
            if n-1 < 1
                ki = Medias1(n,3);
            else
                ki = Medias1(n-1,3);
            end
            if n+1 > NumeroSimulacoes-1
                kf = Medias1(n,3);
            else
                kf = Medias1(n+1,3);
            end
            ik = 0.2 * (kf - ki);
        end
    end
    
    if ik > ikmin
        close all
    else           %Se a simulação chegou ao fim vamos apresentar os graficos com os resultados   
        figure(1)
          subplot(2,1,1)
            hold on; grid on
            axis([min(Tempo)-0.05*max(Tempo) 1.05*max(Tempo) 1.2*min(MedBinTorDirX) -1.2*max(MedBinTorDirX)])
            title('Binario no tornozelo na direcção X sem elasticos (Bin VS Tmp)')
            plot(Tempo,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            %plot(Tempo,MedBin(MelhorSimulacao , :),'COLOR',[0 0 1],'linewidth',2)
            line([max(Tempo)/2 max(Tempo)/2],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
        
          subplot(2,1,2)
            hold on; grid on;
            axis([min(MedAngTorDirX)-0.05*max(MedAngTorDirX) 1.05*max(MedAngTorDirX) 1.2*min(MedBinTorDirX) -1.2*max(MedBinTorDirX)])
            title('Binario no tornozelo na direcção X sem elasticos (Bin VS Ang)')
            plot(MedAngTorDirX,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            %plot(MedAngTorDirX,MedBin(MelhorSimulacao , :),'COLOR',[0 0 1],'linewidth',2)
            line([max(AngTorDirX(:,2)) max(AngTorDirX(:,2))],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
%##########################################################################
%##########################################################################
        figure(2)
          subplot(2,1,1)
            hold on; grid on
            axis([min(Tempo)-0.05*max(Tempo) 1.05*max(Tempo) 1.2*min(MedBin(MelhorSimulacao , :)) 1.2*max(MedBin(MelhorSimulacao , :))])
            title('Binario no tornozelo na direcção X com elasticos (Bin VS Tmp)')
            %plot(Tempo,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            plot(Tempo,MedBin(MelhorSimulacao , :),'COLOR',[0 1 0],'linewidth',2)
            line([max(Tempo)/2 max(Tempo)/2],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
        
          subplot(2,1,2)
            hold on; grid on;
            axis([min(MedAngTorDirX)-0.05*max(MedAngTorDirX) 1.05*max(MedAngTorDirX) 1.2*min(MedBin(MelhorSimulacao , :)) 1.2*max(MedBin(MelhorSimulacao , :))])
            title('Binario no tornozelo na direcção X com elasticos (Bin VS Ang)')
            %plot(MedAngTorDirX,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            plot(MedAngTorDirX,MedBin(MelhorSimulacao , :),'COLOR',[0 1 0],'linewidth',2)
            line([max(AngTorDirX(:,2)) max(AngTorDirX(:,2))],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
%##########################################################################
%##########################################################################
        figure(3)
          subplot(2,1,1)
            hold on; grid on
            axis([min(Tempo)-0.05*max(Tempo) 1.05*max(Tempo) 1.2*min(MedBinTorDirX) 1.2*max(MedBin(MelhorSimulacao , :))])
            title('Binario no tornozelo na direcção X sem elasticos e com elasticos (Bin VS Tmp)')
            plot(Tempo,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            plot(Tempo,MedBin(MelhorSimulacao , :),'COLOR',[0 1 0],'linewidth',2)
            line([max(Tempo)/2 max(Tempo)/2],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
        
          subplot(2,1,2)
            hold on; grid on;
            axis([min(MedAngTorDirX)-0.05*max(MedAngTorDirX) 1.05*max(MedAngTorDirX) 1.2*min(MedBinTorDirX) 1.2*max(MedBin(MelhorSimulacao , :))])
            title('Binario no tornozelo na direcção X sem elasticos e com elasticos (Bin VS Ang)')
            plot(MedAngTorDirX,MedBinTorDirX,'COLOR',[0 0 0],'linewidth',2)
            plot(MedAngTorDirX,MedBin(MelhorSimulacao , :),'COLOR',[0 1 0],'linewidth',2)
            line([max(AngTorDirX(:,2)) max(AngTorDirX(:,2))],[1.2*min(MedBinTorDirX) 1.2*max(MedBinTorDirX)],'COLOR',[1 0 0],'linewidth',2)
	end
    
end