% ******** Simulação Humanoide A Subir Degrau (Com Elasticos) ********

%   Este script simula o movimento de subir um degrau pelo robot actuando 
% apenas as juntas de flexao do tornozelo, joelho e anca.
%   A amplitude de movimentos e dada em funcao do valor assumindo para o
% tornozelo de modo a que o agachamento mantenha o robot na vertical.
%   As trajectorias sao dadas no espaço das juntas através de um percurso
% polinomial de quinta ordem.

%% INPUT VARS
clear all
close all
% Parâmetros: altura do degrau, tempo total e numero de pontos por etapa
STEP_HEIGHT = 50;
TOTAL_TIME = 10;
POINTS_PER_STAGE = 1500;
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%% SETUP
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% ********** Setup da Simulacao **********
[Dimensoes, Elasticos, Degrau, Juntas, Torques]...
    =PC_Setup_Simulation(STEP_HEIGHT, TOTAL_TIME, POINTS_PER_STAGE);
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%% SIMULAÇÃO
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
% ********** Etapa 1 **********
[Torques]=PC_Sim_Step_Climb_And_Compile_Data('step_climbing_1',Torques);
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
%% REPRESENTAÇÃO GRÁFICA
% ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
close all
% ********** Torques **********
%% Perna de Apoio
f=figure('Name','Support Leg Torques');
for i=1:6
    %color picking
    switch i
        case 1
            color = 'r';
        case 2
            color = 'g';
        case 3
            color = 'b';
        case 4
            color = 'm';
        case 5
            color = [255 193 37]/255;
        case 6
            color = [125, 38, 205]/255;
    end
    plot(Torques.Time_Vect,Torques.Perna_Apoio.Junta(i).Torque,'Color', color, 'LineWidth',1.5);
    xlabel('Time [s]'); ylabel('Torque [Nm]');
    grid on; hold on; drawnow
end
drawnow
title('Support Leg Torques');
axis([0 TOTAL_TIME -5 5]);
set(gca,'XTick',0:0.5:TOTAL_TIME);
set(gca,'YTick',-5:0.5:5);
drawnow
legend(Torques.Perna_Apoio.Junta(1).nome_junta,...
    Torques.Perna_Apoio.Junta(2).nome_junta,...
    Torques.Perna_Apoio.Junta(3).nome_junta,...
    Torques.Perna_Apoio.Junta(4).nome_junta,...
    Torques.Perna_Apoio.Junta(5).nome_junta,...
    Torques.Perna_Apoio.Junta(6).nome_junta);
drawnow
print(f, '-depsc2', sprintf('imgs/support_leg_torques_h%d_t%d.eps', STEP_HEIGHT, TOTAL_TIME));
close(f)
%% Perna Livre
f=figure('Name','Free Leg Torques');
for i=1:6
    %color picking
    switch i
        case 1
            color = 'r';
        case 2
            color = 'g';
        case 3
            color = 'b';
        case 4
            color = 'm';
        case 5
            color = [255 193 37]/255;
        case 6
            color = [125, 38, 205]/255;
    end
    H(i) = plot(Torques.Time_Vect,Torques.Perna_Livre.Junta(i).Torque,'Color', color,'LineWidth',2);
    xlabel('Time [s]'); ylabel('Torque [Nm]');
    grid on; hold on; drawnow
end
title('Free Leg Torques');
legend(Torques.Perna_Livre.Junta(1).nome_junta,...
    Torques.Perna_Livre.Junta(2).nome_junta,...
    Torques.Perna_Livre.Junta(3).nome_junta,...
    Torques.Perna_Livre.Junta(4).nome_junta,...
    Torques.Perna_Livre.Junta(5).nome_junta,...
    Torques.Perna_Livre.Junta(6).nome_junta);
axis([0 TOTAL_TIME -2 2]);
set(gca,'XTick',0:0.5:TOTAL_TIME);
set(gca,'YTick',-2:0.25:2);
drawnow
set(gca,'Xcolor',[0.5 0.5 0.5]);
set(gca,'Ycolor',[0.5 0.5 0.5]);
Caxes = copyobj(gca,gcf);
set(Caxes, 'color', 'none', 'xcolor', 'k', 'xgrid', 'off', 'ycolor','k', 'ygrid','off');
drawnow
print(f, '-depsc2', sprintf('imgs/free_leg_torques_h%d_t%d.eps', STEP_HEIGHT, TOTAL_TIME));
close(f)
%% Tronco
f=figure('Name','Torso Torques');
for i=1:3
    %color picking
    switch i
        case 1
            color = 'r';
        case 2
            color = 'g';
        case 3
            color = 'b';
    end
    H(i) = plot(Torques.Time_Vect,Torques.Tronco.Junta(i).Torque,'Color', color,'LineWidth',2);
    xlabel('Time [s]'); ylabel('Torque [Nm]');
    grid on; hold on; drawnow
end
title('Torso Torques');
legend(Torques.Tronco.Junta(1).nome_junta,...
    Torques.Tronco.Junta(2).nome_junta,...
    Torques.Tronco.Junta(3).nome_junta,'Location','East');
axis([0 TOTAL_TIME -0.05 0.4]);
set(gca,'XTick',0:0.5:TOTAL_TIME);
set(gca,'YTick',-0.05:0.025:0.4);
drawnow
print(f, '-depsc2', sprintf('imgs/torso_torques_h%d_t%d.eps', STEP_HEIGHT, TOTAL_TIME));
close(f)
%% Braço Direito
f=figure('Name','Right Arm Torques');
for i=1:4
    %color picking
    switch i
        case 1
            color = 'r';
        case 2
            color = 'g';
        case 3
            color = 'b';
        case 4
            color = [255 193 37]/255;
    end
    H(i) = plot(Torques.Time_Vect,Torques.Braco_Direito.Junta(i).Torque,'Color', color,'LineWidth',2);
    xlabel('Time [s]'); ylabel('Torque [Nm]');
    grid on; hold on; drawnow
end
title('Right Arm Torques');
legend(Torques.Braco_Direito.Junta(1).nome_junta,...
    Torques.Braco_Direito.Junta(2).nome_junta,...
    Torques.Braco_Direito.Junta(3).nome_junta,...
    Torques.Braco_Direito.Junta(4).nome_junta);
axis([0 TOTAL_TIME -0.003 0.003]);
set(gca,'XTick',0:0.5:TOTAL_TIME);
set(gca,'YTick',-0.003:0.00025:0.003);
drawnow
pause
print(f, '-depsc2', sprintf('imgs/right_arm_torques_h%d_t%d.eps', STEP_HEIGHT, TOTAL_TIME));
close(f)
%% Braço Esquerdo
f=figure('Name','Left Arm Torques');
for i=1:4
    %color picking
    switch i
        case 1
            color = 'r';
        case 2
            color = 'g';
        case 3
            color = 'b';
        case 4
            color = [255 193 37]/255;
    end
    H(i) = plot(Torques.Time_Vect,Torques.Braco_Esquerdo.Junta(i).Torque,'Color', color,'LineWidth',2);
    xlabel('Time [s]'); ylabel('Torque [Nm]');
    grid on; hold on; drawnow
end
title('Left Arm Torques');
legend(Torques.Braco_Esquerdo.Junta(1).nome_junta,...
    Torques.Braco_Esquerdo.Junta(2).nome_junta,...
    Torques.Braco_Esquerdo.Junta(3).nome_junta,...
    Torques.Braco_Esquerdo.Junta(4).nome_junta);
axis([0 TOTAL_TIME -0.0005 0.004]);
set(gca,'XTick',0:0.5:TOTAL_TIME);
set(gca,'YTick',-0.0005:0.00025:0.004);
drawnow
pause
print(f, '-depsc2', sprintf('imgs/left_arm_torques_h%d_t%d.eps', STEP_HEIGHT, TOTAL_TIME));
close(f)
%% PLOT SUPPORT LEG TORQUE AND DYNAMIC INFORMATION
 for i=1:6
    f=figure('name',sprintf('Support Leg %s Comparison',...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).nome_junta));
    % pos, vel and ace
    subplot(2,1,1,'align')
    H(1) = plot(Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_pos(:,1),...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_pos(:,2),...
        'r', 'LineWidth', 2);
    hold on
    H(2) = plot(Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_vel(:,1),...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_vel(:,2),...
        'g', 'LineWidth', 2);
    hold on
    H(3) = plot(Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_ace(:,1),...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).input_ace(:,2),...
        'b', 'LineWidth', 2);
    title(sprintf('%s Angular Position, Speed and Acceleration',...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).nome_junta));
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on
    ylabel('Degree')
    drawnow
    % torque
    subplot(2,1,2,'align')
    H(4) = plot(Torques.Time_Vect,Torques.Perna_Apoio.Junta(i).Torque,...
        'Color',[125, 38, 205]/255, 'LineWidth', 1.5);
    title(sprintf('%s Torque',...
        Juntas(1).Perna_Apoio(1).Input_Modelo(i).nome_junta));
    legend(H(4),...
        'Angular Position [^{o}]',...
        'Angular Velocity [^{o}/s]',...
        'Angular Acceleration [^{o}/s^2]',...
        'Developed Torque [N/m]');
    xlabel('Time [s]');ylabel('Torque[Nm]')
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on
    drawnow
    pause
    print(f, '-depsc2', sprintf('imgs/support_leg_comp_%d_h%d_t%d.eps', i, STEP_HEIGHT, TOTAL_TIME));
    close(f)
 end
%% PLOT FREE LEG TORQUE AND DYNAMIC INFORMATION
 for i=1:6
    f=figure('name',sprintf('Free Leg %s Comparison',...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).nome_junta));
    % pos, vel and ace
    subplot(2,1,1,'align')
    H(1) = plot(Juntas(1).Perna_Livre(1).Input_Modelo(i).input_pos(:,1),...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).input_pos(:,2),...
        'r', 'LineWidth', 2);
    hold on
    H(2) = plot(Juntas(1).Perna_Livre(1).Input_Modelo(i).input_vel(:,1),...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).input_vel(:,2),...
        'g', 'LineWidth', 2);
    hold on
    H(3) = plot(Juntas(1).Perna_Livre(1).Input_Modelo(i).input_ace(:,1),...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).input_ace(:,2),...
        'b', 'LineWidth', 2);
    title(sprintf('%s Angular Position, Speed and Acceleration',...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).nome_junta));
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on; ylabel('Degree')
    drawnow
    % torque
    subplot(2,1,2,'align')
    H(4) = plot(Torques.Time_Vect,Torques.Perna_Livre.Junta(i).Torque,...
        'Color',[125, 38, 205]/255, 'LineWidth', 1.5);
     title(sprintf('%s Torque',...
        Juntas(1).Perna_Livre(1).Input_Modelo(i).nome_junta));
    legend([H(1),H(2),H(3),H(4)],...
        'Angular Position [^{o}]',...
        'Angular Velocity [^{o}/s]',...
        'Angular Acceleration [^{o}/s^2]',...
        'Developed Torques [N/m]');
    xlabel('Time [s]');ylabel('Torque[Nm]')
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on
    drawnow
    pause
    print(f, '-depsc2', sprintf('imgs/free_leg_comp_%d_h%d_t%d.eps', i, STEP_HEIGHT, TOTAL_TIME));
    close(f)
 end
 %% PLOT TORSO TORQUE AND DYNAMIC INFORMATION
 for i=1:3
    f=figure('name',sprintf('Free Leg %s Comparison',...
        Juntas(1).Tronco(1).Input_Modelo(i).nome_junta));
    % pos, vel and ace
    subplot(2,1,1,'align')
    H(1) = plot(Juntas(1).Tronco(1).Input_Modelo(i).input_pos(:,1),...
        Juntas(1).Tronco(1).Input_Modelo(i).input_pos(:,2),...
        'r', 'LineWidth', 2);
    hold on
    H(2) = plot(Juntas(1).Tronco(1).Input_Modelo(i).input_vel(:,1),...
        Juntas(1).Tronco(1).Input_Modelo(i).input_vel(:,2),...
        'g', 'LineWidth', 2);
    hold on
    H(3) = plot(Juntas(1).Tronco(1).Input_Modelo(i).input_ace(:,1),...
        Juntas(1).Tronco(1).Input_Modelo(i).input_ace(:,2),...
        'b', 'LineWidth', 2);
    title(sprintf('%s Angular Position, Speed and Acceleration',...
        Juntas(1).Tronco(1).Input_Modelo(i).nome_junta));
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on; ylabel('Degree')
    drawnow
    % torque
    subplot(2,1,2,'align')
    H(4) = plot(Torques.Time_Vect,Torques.Tronco.Junta(i).Torque,...
        'Color',[125, 38, 205]/255, 'LineWidth', 1.5);
     title(sprintf('%s Torque',...
        Juntas(1).Tronco(1).Input_Modelo(i).nome_junta));
    legend([H(1),H(2),H(3),H(4)],...
        'Angular Position [^{o}]',...
        'Angular Velocity [^{o}/s]',...
        'Angular Acceleration [^{o}/s^2]',...
        'Developed Torques [N/m]');
    xlabel('Time [s]');ylabel('Torque[Nm]')
    set(gca,'XTick',0:0.5:TOTAL_TIME);
    grid on
    pause
    drawnow
    print(f, '-depsc2', sprintf('imgs/torso_comp_%d_h%d_t%d.eps', i, STEP_HEIGHT, TOTAL_TIME));
    close(f)
 end