%% RAW DATA (from bag's) to graphs
clear all; close all; clc

addpath export_fig

% load MoveXX_2013-06-21-11-23-48.mat
% load MoveXX_force_2013-06-25-15-05-48.mat   %% TESE
% load MoveYY_2013-06-21-11-47-40.mat
% load MoveYY_force_2013-06-25-15-16-23.mat   %% TESE
% load MoveZZ_2013-06-21-11-37-16.mat
% load MoveZZ_noforce_2013-06-25-11-37-46.mat
% load MoveZZ_noforce_2013-06-25-11-41-33.mat
load MoveZZ_noforce_2013-06-25-14-28-54.mat   %% TESE
% load MoveZZ_force_2013-06-25-22-57-25.mat

arduino1_seq=arduino_1_values(:,1);
arduino2_seq=arduino_2_values(:,1);
cop_L_seq=cop_left(:,1);
cop_LR_seq=cop_left_right(:,1);
cop_R_seq=cop_right(:,1);
% force_seq=force(:,1);
humanoid_seq=humanoid_state(:,1);
phantom_seq=phantom_state(:,1);

arduino1_time=arduino_1_values(:,2);
arduino2_time=arduino_2_values(:,2);
cop_L_time=cop_left(:,2);
cop_LR_time=cop_left_right(:,2);
cop_R_time=cop_right(:,2);
% force_time=force(:,2);
humanoid_time=humanoid_state(:,2);
phantom_time=phantom_state(:,2);

arduino1_time=arduino1_time-arduino1_time(1);
arduino2_time=arduino2_time-arduino2_time(1);
cop_L_time=cop_L_time-cop_L_time(1);
cop_LR_time=cop_LR_time-cop_LR_time(1);
cop_R_time=cop_R_time-cop_R_time(1);
% force_time=force_time-force_time(1);
humanoid_time=humanoid_time-humanoid_time(1);
phantom_time=phantom_time-phantom_time(1);

arduino1_values=arduino_1_values(:,3:end);
arduino2_values=arduino_2_values(:,3:end);
cop_L_values=cop_left(:,3:end);
cop_LR_values=cop_left_right(:,3:end);
cop_R_values=cop_right(:,3:end);
% force_values=force(:,3:end);
humanoid_values=humanoid_state(:,3:end);
phantom_values=phantom_state(:,3:end);

% s_start=39;  %% Move YY
s_start=0;
% s_final=max([arduino1_time' arduino2_time' cop_L_time' cop_LR_time' cop_R_time' force_time' humanoid_time' phantom_time']);
s_final=28;
%%
close all; clc

i_start_ard1=find(arduino1_time>=s_start);
i_final_ard1=find(arduino1_time<=s_final);

h1=figure;
set(h1,'Name','arduino1_values','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

% [B,A] = butter(1,0.001,'low');
% ard1_1=filter(B,A,arduino1_values(:,1))

plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)),arduino1_values(i_start_ard1(1):i_final_ard1(end),1),'r','LineWidth',1)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)),arduino1_values(i_start_ard1(1):i_final_ard1(end),2),'b','LineWidth',1)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)),arduino1_values(i_start_ard1(1):i_final_ard1(end),3),'g','LineWidth',1)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)),arduino1_values(i_start_ard1(1):i_final_ard1(end),4),'k','LineWidth',1)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[arduino1_time(i_start_ard1(1)) arduino1_time(i_final_ard1(end))],'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Force (N)')
xlabel('Time (s)')

set(gca,'FontSize',20)
legend( 'Cell 1', 'Cell 2', 'Cell 3', 'Cell 4','Location','Best')

%  print( h1, '-dpdf','-r1200', 'arduino1_values.pdf')
export_fig 'arduino1_values.pdf' -pdf

i_start_ard2=find(arduino2_time>=s_start);
i_final_ard2=find(arduino2_time<=s_final);

h2=figure;
set(h2,'Name','arduino2_values','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)),arduino2_values(i_start_ard2(1):i_final_ard2(end),1),'r','LineWidth',1)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)),arduino2_values(i_start_ard2(1):i_final_ard2(end),2),'b','LineWidth',1)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)),arduino2_values(i_start_ard2(1):i_final_ard2(end),3),'g','LineWidth',1)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)),arduino2_values(i_start_ard2(1):i_final_ard2(end),4),'k','LineWidth',1)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[arduino2_time(i_start_ard2(1)) arduino2_time(i_final_ard2(end))],'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Force (N)')
xlabel('Time (s)')

set(gca,'FontSize',20)
legend( 'Cell 1', 'Cell 2', 'Cell 3', 'Cell 4','Location','Best')

%  print( h2, '-dpdf','-r1200', 'arduino2_values.pdf')
export_fig 'arduino2_values.pdf' -pdf

%%
close all; clc

h3=figure;
set(h3,'Name','cop_LR_values','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

plot(cop_LR_values(:,2)-0.0700,cop_LR_values(:,1),'r','LineWidth',0.5)

rectangle('Position',[-0.023-0.0700 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

     
rectangle('Position',[-0.023+0.14-0.0700 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
text(0-0.0700,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14-0.0700,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0-0.0700,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14-0.0700,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
ee_circle(0.018-0.0700,-0.04,0.004,'k',2)
 ee_circle(-0.018-0.0700,-0.04,0.004,'k',2)
 ee_circle(0.018-0.0700,0.038,0.004,'k',2)
 ee_circle(-0.018-0.0700,0.038,0.004,'k',2)
 
 text(0.018-0.0700,-0.040,'4',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018-0.0700,-0.040,'3',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
  text(0.018-0.0700,0.038,'1',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018-0.0700,0.038,'2',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
ee_circle(0.018+0.0700,-0.04,0.004,'k',2)
 ee_circle(-0.018+0.0700,-0.04,0.004,'k',2)
 ee_circle(0.018+0.0700,0.038,0.004,'k',2)
 ee_circle(-0.018+0.0700,0.038,0.004,'k',2)
 
  text(0.018+0.0700,-0.040,'4',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018+0.0700,-0.040,'3',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
  text(0.018+0.0700,0.038,'1',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018+0.0700,0.038,'2',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ... ,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ,'YLim',[-0.055 0.05]...
    ,'XDir','reverse')

ylabel('X (m)')
xlabel('Y (m)')


%  print( h3, '-dpdf','-r1200', 'cop_LR_values.pdf')
export_fig 'cop_LR_values.pdf' -pdf


%%
close all; clc

i_start_cop=find(cop_LR_time>=s_start);
i_final_cop=find(cop_LR_time<=s_final);

h4=figure;
set(h4,'Name','cop_LR_axis_values','Color',[1,1,1], 'Position' ,[100 100 1200 640],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

subplot(2,1,1)
plot(cop_LR_time(i_start_cop(1):i_final_cop(end),1) - cop_LR_time(i_start_cop(1)),cop_LR_values(i_start_cop(1):i_final_cop(end),1),'r','LineWidth',2)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[cop_LR_time(i_start_cop(1)) cop_LR_time(i_final_cop(end))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('X (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(cop_LR_time(i_start_cop(1):i_final_cop(end),1) - cop_LR_time(i_start_cop(1)),cop_LR_values(i_start_cop(1):i_final_cop(end),2)-0.0700,'b','LineWidth',2)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[cop_LR_time(i_start_cop(1)) cop_LR_time(i_final_cop(end))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Y (m)')
xlabel('Time (s)')


%  print( h4, '-dpdf','-r1200', 'cop_LR_axis_values.pdf')
export_fig 'cop_LR_axis_values.pdf' -pdf
 
% % subplot(2,1,1)
% % plot(cop_LR_time(size(cop_LR_values_F,1)-size(cop_LR_values,1)+1:end),cop_LR_values_F(:,1),'r')
% % subplot(2,1,2)
% % plot(cop_LR_time(size(cop_LR_values_F,1)-size(cop_LR_values,1)+1:end),cop_LR_values_F(:,2),'b')

%%
close all; clc

i_start_hum=find(humanoid_time>=s_start);
i_final_hum=find(humanoid_time<=s_final);

i_start_pha=find(phantom_time>=s_start);
i_final_pha=find(phantom_time<=s_final);

% humanoid_values_int=interp1(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),:),phantom_time(i_start_pha(1):i_final_pha(end),1));

h5=figure;
set(h5,'Name','humanoide_joints_time','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

% Move ZZ
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+2),'r','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+3),'r','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+4),'b','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+5),'b','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+8),'g','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1),humanoid_values(i_start_hum(1):i_final_hum(end),13+9),'g','LineWidth',2)

% Move YY
plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+0),'r','LineWidth',2)
plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+1),'r','LineWidth',2)
plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+6),'b','LineWidth',2)
plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+7),'b','LineWidth',2)

% Move XX
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+4),'r','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+5),'r','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+8),'b','LineWidth',2)
% plot(humanoid_time(i_start_hum(1):i_final_hum(end),1) - humanoid_time(i_start_hum(1)),humanoid_values(i_start_hum(1):i_final_hum(end),13+9),'b','LineWidth',2)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(cop_LR_time) max(cop_LR_time)]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Joint angles (\circ)')
xlabel('Time (s)')

set(gca,'FontSize',20)
legend( 'Ankle joint', 'Knee joint', 'Hip joint','Location','Best')

%  print( h5, '-dpdf','-r1200', 'humanoide_joints_time.pdf')
export_fig 'humanoide_joints_time.pdf' -pdf


%%
%
close all; clc

i_start_force=find(force_time>=s_start);
i_final_force=find(force_time<=s_final);

h7=figure;
set(h7,'Name','cop_x_axis_w/_force_x_axis','Color',[1,1,1], 'Position' ,[100 100 1200 640],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

subplot(2,1,1)
plot(cop_LR_time(i_start_cop(1):i_final_cop(end),1) - cop_LR_time(i_start_cop(1)),cop_LR_values(i_start_cop(1):i_final_cop(end),1),'r','LineWidth',2)

line([ -1 50], [0.0138 0.0138],'Color','k','LineWidth',3)
line([ -1 50], [-0.0138 -0.0138],'Color','k','LineWidth',3)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25,'XLim',[min(cop_LR_time(i_start_cop(1))) - cop_LR_time(i_start_cop(1)) max(cop_LR_time(i_final_cop(end))) - cop_LR_time(i_start_cop(1))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('COP X (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(force_time(i_start_force(1):i_final_force(end)) - force_time(i_start_force(1)),force_values(i_start_force(1):i_final_force(end),3),'b','LineWidth',2)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(force_time(i_start_force(1))) max(force_time(i_final_force(end)))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Force X (N)')
xlabel('Time (s)')

export_fig 'cop_x_axis_w_force_x_axis.pdf' -pdf


h8=figure;
set(h8,'Name','cop_y_axis_w/_force_y_axis','Color',[1,1,1], 'Position' ,[100 100 1200 640],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

subplot(2,1,1)
plot(cop_LR_time(i_start_cop(1):i_final_cop(end),1) - cop_LR_time(i_start_cop(1)),cop_LR_values(i_start_cop(1):i_final_cop(end),2)-0.0700,'r','LineWidth',2)

line([ -1 50], [0.091-0.0700 0.091-0.0700],'Color','k','LineWidth',3)
line([ -1 50], [0.049-0.0700 0.049-0.0700],'Color','k','LineWidth',3)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ,'XLim',[min(cop_LR_time(i_start_cop(1))) - cop_LR_time(i_start_cop(1)) max(cop_LR_time(i_final_cop(end))) - cop_LR_time(i_start_cop(1))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ,'YLim',[-0.06 0.06])

ylabel('COP Y (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(force_time(i_start_force(1):i_final_force(end)) - force_time(i_start_force(1)),force_values(i_start_force(1):i_final_force(end),1),'b','LineWidth',2)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(force_time(i_start_force(1))) max(force_time(i_final_force(end)))]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Force Y (N)')
xlabel('Time (s)')

export_fig 'cop_y_axis_w_force_y_axis.pdf' -pdf


%%
close all; clc


h9=figure;
set(h9,'Name','phantom_dimension','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on
plot(phantom_time(i_start_pha(1):i_final_pha(end),1) - phantom_time(i_start_pha(1)),phantom_values(i_start_pha(1):i_final_pha(end),1),'r','LineWidth',1)

set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[phantom_time(i_start_pha(1),1) phantom_time(i_final_pha(end),1)]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

ylabel('Phantom''s Y (mm)')
xlabel('Time (s)')

%  print( h9, '-dpdf','-r1200', 'phantom_dimension.pdf')
export_fig 'phantom_dimension.pdf' -pdf

%%
close all; clc

h10=figure;
set(h10,'Name','phantom_joints_time','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

phantom_human_time=sort([humanoid_time' phantom_time'])';
humanoid_values_int=interp1(humanoid_time,humanoid_values,phantom_human_time);
phantom_values_int=interp1(phantom_time,phantom_values,phantom_human_time);

i_start_hum_pha=find(phantom_human_time>=s_start);
i_final_hum_pha=find(phantom_human_time<=s_final);

% Move ZZ
% [ax, h1, h2] = plotyy(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+2),phantom_human_time(i_start_pha(1):i_final_pha(end),1),phantom_values_int(i_start_pha(1):i_final_pha(end),2));
% 
% % plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+2),'r','LineWidth',2)
% % plot(phantom_human_time(i_start_hum(1):i_final_hum(end),1),humanoid_values_int(i_start_hum(1):i_final_hum(end),13+3),'r','LineWidth',2)
% plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+4),'b','LineWidth',2)
% % plot(phantom_human_time(i_start_hum(1):i_final_hum(end),1),humanoid_values_int(i_start_hum(1):i_final_hum(end),13+5),'b','LineWidth',2)
% plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+8),'g','LineWidth',2)

% Move YY
% [ax, h1, h2] = plotyy(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1) - phantom_human_time(i_start_hum_pha(1)),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+0),phantom_human_time(i_start_pha(1):i_final_pha(end),1) - phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),1));
% 
% % plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+1),'b','LineWidth',2)
% plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1) - phantom_human_time(i_start_hum_pha(1)),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+6),'b','LineWidth',2)
% % plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+7),'k','LineWidth',2)

% Move XX
[ax, h1, h2] = plotyy(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1) - phantom_human_time(i_start_hum_pha(1)),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+4),phantom_human_time(i_start_pha(1):i_final_pha(end),1) - phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),3));

% plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+1),'b','LineWidth',2)
plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1) - phantom_human_time(i_start_hum_pha(1)),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+8),'b','LineWidth',2)
% plot(phantom_human_time(i_start_hum_pha(1):i_final_hum_pha(end),1),humanoid_values_int(i_start_hum_pha(1):i_final_hum_pha(end),13+7),'k','LineWidth',2)

% set(gca, 'LooseInset',[0.1 0.1 0.1 0.1])
set(gca,'box','off','FontSize',25)

xlabel('Time (s)')

set(get(ax(1),'Ylabel'),'String','Joint angles (\circ)','FontSize',25,'color','k')
set(get(ax(2),'Ylabel'),'String','Phantom''s X (mm)','FontSize',25,'color','k' ...,'Units', 'Normalized', 'Position', [1.05 0.666]
    ) 
set(ax(1),'YColor','k','YLim',[-30 80],'YTick', -30:10:80)
set(ax(2),'YColor','k','FontSize',25)

set(h1,'Color','r','LineWidth',2)
set(h2,'Color','k','LineWidth',2,'LineStyle','--')

hleg1 =legend( 'Ankle joint'...,
    ...'Knee joint'...
    , 'Hip joint','X axis'...,'Position',[300 120 50 50]
    ,'Location','Best');

set(hleg1,'FontSize',20);

export_fig 'phantom_joints_time.pdf' -pdf
%%
close all; clc

h11=figure;
set(h11,'Name','humanoide_ard1_time','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

mov_size  = 200; % para func moving_average

% phantom_human_time=sort([humanoid_time' phantom_time'])';
% humanoid_values_int=interp1(humanoid_time,humanoid_values,phantom_human_time);
% phantom_values_int=interp1(phantom_time,phantom_values,phantom_human_time);
% 
% i_start_hum_pha=find(phantom_human_time>=s_start);
% i_final_hum_pha=find(phantom_human_time<=s_final);

% Move ZZ
% [ax, h1, h2] = plotyy(arduino1_time(i_start_ard1(1):i_final_ard1(end),1), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),1), mov_size),phantom_human_time(i_start_pha(1):i_final_pha(end),1),phantom_values_int(i_start_pha(1):i_final_pha(end),2));

% Move YY
[ax, h1, h2] = plotyy(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),1), mov_size),phantom_human_time(i_start_pha(1):i_final_pha(end),1) - phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),3));

% Move XX
% [ax, h1, h2] = plotyy(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),1), mov_size),phantom_human_time(i_start_pha(1):i_final_pha(end),1) - phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),3));

% plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1),arduino1_values(i_start_ard1(1):i_final_ard1(end),1),'r','LineWidth',1)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),2), mov_size),'b','LineWidth',2)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),3), mov_size),'g','LineWidth',2)
plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)), moving_average(arduino1_values(i_start_ard1(1):i_final_ard1(end),4), mov_size),'k','LineWidth',2)

set(gca,'box','off','FontSize',25)

xlabel('Time (s)')

set(get(ax(1),'Ylabel'),'String','Force (N)','FontSize',25,'color','k')
set(get(ax(2),'Ylabel'),'String','Phantom''s Y (mm)','FontSize',25,'color','k'...,'Units', 'Normalized', 'Position', [1.05 0.5]
    );
% set(y, 'Position', [-0.1, 0.5, 0]);
set(ax(1),'YColor','k','YLim',[0 20],'YTick',[-20 -10 0 5 10 15 20 30 40 50])
set(ax(2),'YColor','k','YLim',[-50 50],'YTick',[-50 0 50]...
    ,'FontSize',25)

set(h1,'Color','r','LineWidth',2)
set(h2,'Color','k','LineWidth',2,'LineStyle','--')

hleg1 =legend( 'Cell 1', 'Cell 2', 'Cell 3','Cell 4','Y axis','Location','Best');

set(hleg1,'FontSize',20);

export_fig 'humanoide_ard1_time.pdf' -pdf

%
% close all; clc

h11=figure;
set(h11,'Name','humanoide_ard2_time','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

% phantom_human_time=sort([humanoid_time' phantom_time'])';
% humanoid_values_int=interp1(humanoid_time,humanoid_values,phantom_human_time);
% phantom_values_int=interp1(phantom_time,phantom_values,phantom_human_time);
% 
% i_start_hum_pha=find(phantom_human_time>=s_start);
% i_final_hum_pha=find(phantom_human_time<=s_final);

[ax, h1, h2] = plotyy(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),1), mov_size),phantom_human_time(i_start_pha(1):i_final_pha(end),1) - phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),3));


% plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1),arduino1_values(i_start_ard1(1):i_final_ard1(end),1),'r','LineWidth',1)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),2), mov_size),'b','LineWidth',2)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),3), mov_size),'g','LineWidth',2)
plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),4), mov_size),'k','LineWidth',2)

% plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1), moving_average(sum(arduino2_values(i_start_ard2(1):i_final_ard2(end),:),2), mov_size),'r--','LineWidth',2)

% set(gca, 'LooseInset',[0.1 0.1 0.1 0.1])
set(gca,'box','off','FontSize',25)

xlabel('Time (s)')

set(get(ax(1),'Ylabel'),'String','Force (N)','FontSize',25,'color','k')
set(get(ax(2),'Ylabel'),'String','Phantom''s Y (mm)','FontSize',25,'color','k'...,'Units', 'Normalized', 'Position', [1.05 0.5]
    );
% set(y, 'Position', [-0.1, 0.5, 0]);
set(ax(1),'YColor','k','YLim',[0 20],'YTick',[-20 -10 0 5 10 15 20 30 40 50])
set(ax(2),'YColor','k','YLim',[-50 50],'YTick',[-50 0 50]...
    ,'FontSize',25)

set(h1,'Color','r','LineWidth',2)
set(h2,'Color','k','LineWidth',2,'LineStyle','--')

hleg1 =legend( 'Cell 1', 'Cell 2', 'Cell 3','Cell 4'...,'Sum of Cells'
    ,'Y axis','Location','Best');

set(hleg1,'FontSize',20);

export_fig 'humanoide_ard2_time.pdf' -pdf

%%
close all; clc

h12=figure;
set(h12,'Name','humanoide_total_ard_1_2_time','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

% phantom_human_time=sort([humanoid_time' phantom_time'])';
% humanoid_values_int=interp1(humanoid_time,humanoid_values,phantom_human_time);
% phantom_values_int=interp1(phantom_time,phantom_values,phantom_human_time);
% 
% i_start_hum_pha=find(phantom_human_time>=s_start);
% i_final_hum_pha=find(phantom_human_time<=s_final);

% Move ZZ
% [ax, h1, h2] = plotyy(arduino1_time(i_start_ard1(1):i_final_ard1(end),1), moving_average(sum(arduino1_values(i_start_ard1(1):i_final_ard1(end),:),2), mov_size),phantom_human_time(i_start_pha(1):i_final_pha(end),1),phantom_values_int(i_start_pha(1):i_final_pha(end),2));

% Move YY
[ax, h1, h2] = plotyy(arduino1_time(i_start_ard1(1):i_final_ard1(end),1) - arduino1_time(i_start_ard1(1)) , moving_average(sum(arduino1_values(i_start_ard1(1):i_final_ard1(end),:),2), mov_size), phantom_human_time(i_start_pha(1):i_final_pha(end),1) -  phantom_human_time(i_start_pha(1)),phantom_values_int(i_start_pha(1):i_final_pha(end),3));


% plot(arduino1_time(i_start_ard1(1):i_final_ard1(end),1),arduino1_values(i_start_ard1(1):i_final_ard1(end),1),'r','LineWidth',1)
% plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),2), mov_size),'b','LineWidth',1.5)
% plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),3), mov_size),'g','LineWidth',1.5)
% plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1), moving_average(arduino2_values(i_start_ard2(1):i_final_ard2(end),4), mov_size),'k','LineWidth',1.5)

plot(arduino2_time(i_start_ard2(1):i_final_ard2(end),1) - arduino2_time(i_start_ard2(1)), moving_average(sum(arduino2_values(i_start_ard2(1):i_final_ard2(end),:),2), mov_size),'b','LineWidth',1.5)

% set(gca, 'LooseInset',[0.1 0.1 0.1 0.1])
set(gca,'box','off','FontSize',25)

xlabel('Time (s)')

set(get(ax(1),'Ylabel'),'String','Force (N)','FontSize',25,'color','k')
set(get(ax(2),'Ylabel'),'String','Phantom''s X (mm)','FontSize',25,'color','k' ...,'Units', 'Normalized', 'Position', [1.05 0.666] 
    );
% set(y, 'Position', [-0.1, 0.5, 0]);
set(ax(1),'YColor','k','YLim',[0 50],'YTick',[-20 -10 0 10 20 30 40 50])

set(ax(2),'YColor','k'...,'YLim',[-150 150],'YTick',[-100 0 100]
    ,'FontSize',25)

set(h1,'Color','r','LineWidth',1.5)
set(h2,'Color','k','LineWidth',2,'LineStyle','--')

hleg1 =legend(  'Total forces left foot','Total forces right foot'...,'Sum of Cells'
    ,'X axis','Location','Best');
set(hleg1,'FontSize',20);

export_fig 'humanoide_total_ard_1_2_time.pdf' -pdf

%%
close all; clc


i_1_cop=find(cop_LR_time>=s_start);
i_2_cop=find(cop_LR_time<=10);
i_3_cop=find(cop_LR_time<=18);
i_4_cop=find(cop_LR_time<=26.2);
i_final_cop=find(cop_LR_time<=s_final);


h13=figure;
set(h13,'Name','cop_LR_stage1','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])

plot(cop_LR_values(i_1_cop(1):i_2_cop(end),2),cop_LR_values(i_1_cop(1):i_2_cop(end),1),'r','LineWidth',0.5)

rectangle('Position',[-0.023 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

     
rectangle('Position',[-0.023+0.14 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

text(0,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ...,'YLim',[min(cop_LR_values(:,1))-0.0075 max(cop_LR_values(:,1))+0.0075]
    ,'XDir','reverse')

ylabel('X (m)')
xlabel('Y (m)')

export_fig 'cop_LR_stage1.pdf' -pdf

h14=figure;
set(h14,'Name','cop_LR_stage2','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])

plot(cop_LR_values(i_2_cop(end):i_3_cop(end),2),cop_LR_values(i_2_cop(end):i_3_cop(end),1),'r','LineWidth',0.5)

rectangle('Position',[-0.023 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

     
rectangle('Position',[-0.023+0.14 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
text(0,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ...,'YLim',[min(cop_LR_values(:,1))-0.0075 max(cop_LR_values(:,1))+0.0075]
    ,'XDir','reverse')

ylabel('X (m)')
xlabel('Y (m)')

export_fig 'cop_LR_stage2.pdf' -pdf

h15=figure;
set(h15,'Name','cop_LR_stage3','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])

plot(cop_LR_values(i_3_cop(end):i_4_cop(end),2),cop_LR_values(i_3_cop(end):i_4_cop(end),1),'r','LineWidth',0.5)

rectangle('Position',[-0.023 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

     
rectangle('Position',[-0.023+0.14 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
text(0,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ...,'YLim',[min(cop_LR_values(:,1))-0.0075 max(cop_LR_values(:,1))+0.0075]
    ,'XDir','reverse')

ylabel('X (m)')
xlabel('Y (m)')

export_fig 'cop_LR_stage3.pdf' -pdf

h16=figure;
set(h16,'Name','cop_LR_stage4','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])

plot(cop_LR_values(i_4_cop(end):i_final_cop(end),2),cop_LR_values(i_4_cop(end):i_final_cop(end),1),'r','LineWidth',0.5)

rectangle('Position',[-0.023 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')

     
rectangle('Position',[-0.023+0.14 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
text(0,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
set(gca, 'LooseInset',[0.08 0.08 0.08 0.08])  % diminuir a caixa à volta
set(gca,'box','off','FontSize',25 ...,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ...,'YLim',[min(cop_LR_values(:,1))-0.0075 max(cop_LR_values(:,1))+0.0075]
    ,'XDir','reverse')

ylabel('X (m)')
xlabel('Y (m)')


%  print( h4, '-dpdf','-r1200', 'cop_LR_axis_values.pdf')
export_fig 'cop_LR_stage4.pdf' -pdf

%% 


h17=figure;
set(h17,'Name','cells','Color',[1,1,1], 'Position' ,[100 100 980 480],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on

rectangle('Position',[-0.023-0.0700 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
     
rectangle('Position',[-0.023+0.14-0.0700 -0.046 0.023*2 0.044+0.046 ],...
        'curvature',[0.2 0.1],...
         'LineWidth',2,'LineStyle','-')
text(0-0.0700,-0.04,'Right',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14-0.0700,-0.04,'Left',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(0-0.0700,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
text(0.14-0.0700,0.038,'Front',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
ee_circle(0.018-0.0700,-0.04,0.004,'k',2,[60/255 110/255 1])
ee_circle(-0.018-0.0700,-0.04,0.004,'k',2,[1 65/255 65/255])
ee_circle(0.018-0.0700,0.038,0.004,'k',2,[60/255 110/255 1])
ee_circle(-0.018-0.0700,0.038,0.004,'k',2,[1 65/255 65/255])

 
ee_circle(0.018+0.0700,-0.04,0.004,'k',2,[1 65/255 65/255])
ee_circle(-0.018+0.0700,-0.04,0.004,'k',2,[60/255 110/255 1])
ee_circle(0.018+0.0700,0.038,0.004,'k',2,[1 65/255 65/255])
ee_circle(-0.018+0.0700,0.038,0.004,'k',2,[60/255 110/255 1])


 text(0.018-0.0700,-0.040,'4',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018-0.0700,-0.040,'3',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
  text(0.018-0.0700,0.038,'1',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018-0.0700,0.038,'2',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
  text(0.018+0.0700,-0.040,'4',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018+0.0700,-0.040,'3',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
  text(0.018+0.0700,0.038,'1',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 text(-0.018+0.0700,0.038,'2',... 
     'HorizontalAlignment','center',...
     'FontSize',20)
 
set(gca,'box','off','FontSize',25 ... ,'XLim',[min(cop_LR_values(:,2))-0.005 max(cop_LR_values(:,2))+0.005]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    ...,'YLim',[-0.055 0.05]...
    ,'XDir','reverse'...
    ,'xtick',[],'ytick',[],'Visible','off')

%  print( h3, '-dpdf','-r1200', 'cop_LR_values.pdf')
export_fig 'cells.pdf' -pdf
