clear all
% close all
clc

data = csvread('Results/recorded_joints_torque.csv');

time = data(:,1);
time = time/100000;
right_joint1 = data(:,2);
right_joint2 = data(:,3);
right_joint3 = data(:,4);
right_joint4 = data(:,5);
right_joint5 = data(:,6);
right_joint6 = data(:,7);

left_joint1 = data(:,8);
left_joint2 = data(:,9);
left_joint3 = data(:,10);
left_joint4 = data(:,11);
left_joint5 = data(:,12);
left_joint6 = data(:,13);

% figure(1)
% plot(time, right_joint1, 'c', 'LineWidth',1);
% hold on
% plot(time, right_joint2, 'm', 'LineWidth',1);
% hold on
% plot(time, right_joint3, 'y', 'LineWidth',1);
% hold on
% plot(time, right_joint4, 'b', 'LineWidth',1);
% hold on
% plot(time, right_joint5, 'g', 'LineWidth',1);
% hold on
% plot(time, right_joint6, 'r', 'LineWidth',1);
% hold on
% 
% legend('right joint 1','right joint 2','right joint 3','right joint 4','right joint 5','right joint 6') 
% 
% xlabel('Time');
% ylabel('Joints torque');
% 
% grid on
% axis auto
% 
% figure(2)
% plot(time, left_joint1, 'c', 'LineWidth',1);
% hold on
% plot(time, left_joint2, 'm', 'LineWidth',1);
% hold on
% plot(time, left_joint3, 'y', 'LineWidth',1);
% hold on
% plot(time, left_joint4, 'b', 'LineWidth',1);
% hold on
% plot(time, left_joint5, 'g', 'LineWidth',1);
% hold on
% plot(time, left_joint6, 'r', 'LineWidth',1);
% 
% legend('left joint 1','left joint 2','left joint 3','left joint 4','left joint 5','left joint 6') 
% 
% xlabel('Time');
% ylabel('Joints torque');
% 
% grid on
% axis auto

figure('position', [80 80 600 600], 'color', [1 1 1])

AxisFontName = 'Helvetica';
AxisFontSize = 8;

subplot(3,2,1)
plot(time, right_joint1, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint1, 'r', 'LineWidth', 0.5);

set(gca, 'XTick', 0:5:60, 'YTick', -01.75:0.25:01.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Hip roll', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -01.75 01.75])

% -----------------------------------------------------------------------

subplot(3,2,3)
plot(time, right_joint2, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint2, 'r', 'LineWidth', 0.5);
 
set(gca, 'XTick', 0:5:60, 'YTick', -01.75:0.25:01.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Hip pitch', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -01.75 01.75])

% -----------------------------------------------------------------------

subplot(3,2,5)
plot(time, right_joint3, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint3, 'r', 'LineWidth', 0.5);

set(gca, 'XTick', 0:5:60, 'YTick', -0.75:0.25:0.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Hip yaw', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -0.75 0.75])

% -----------------------------------------------------------------------

subplot(3,2,2)
plot(time, right_joint4, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint4, 'r', 'LineWidth', 0.5);

set(gca, 'XTick', 0:5:60, 'YTick', -0.75:0.25:2.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Knee roll', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -0.75 2.75])

% -----------------------------------------------------------------------

subplot(3,2,4)
plot(time, right_joint5, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint5, 'r', 'LineWidth', 0.5);

set(gca, 'XTick', 0:5:60, 'YTick', -0.75:0.25:0.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Ankle roll', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -0.75 0.75])

% -----------------------------------------------------------------------

subplot(3,2,6)
plot(time, right_joint6, 'b', 'LineWidth', 0.5);
hold on
plot(time, left_joint6, 'r', 'LineWidth', 0.5);

set(gca, 'XTick', 0:5:60, 'YTick', -0.75:0.25:0.75, 'FontName', AxisFontName, 'FontSize', AxisFontSize)

legend('right joint', 'left joint') 
title('Ankle pitch', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
xlabel('Time [s]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)
ylabel('Torque [Nm]', 'Fontsize', AxisFontSize, 'FontName', AxisFontName)

grid on
axis ([0 90 -0.75 0.75])

export_fig joint_torque -pdf -transparent
