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

% load MoveXX_2013-06-21-11-23-48.mat
% load MoveXX_force_2013-06-25-15-05-48.mat
% load MoveYY_2013-06-21-11-47-40.mat
load MoveYY_force_2013-06-25-15-16-23.mat
% 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
% 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=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=15;
%%
% i_start=find(arduino1_time>=s_start);
% i_final=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
plot(arduino1_time,arduino1_values(:,1),'r','LineWidth',1)
plot(arduino1_time,arduino1_values(:,2),'b','LineWidth',1)
plot(arduino1_time,arduino1_values(:,3),'g','LineWidth',1)
plot(arduino1_time,arduino1_values(:,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',[0 arduino1_time(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','North')



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,arduino2_values(:,1),'r','LineWidth',1)
plot(arduino2_time,arduino2_values(:,2),'b','LineWidth',1)
plot(arduino2_time,arduino2_values(:,3),'g','LineWidth',1)
plot(arduino2_time,arduino2_values(:,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',[0 arduino1_time(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','NorthEastOutSide')

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

plot(cop_LR_values(:,2),cop_LR_values(:,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',[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])

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



% % figure
% % hold on
% % view(120,30)
% % plot3(cop_LR_values(:,2),cop_LR_values(:,1),cop_LR_time)
% % for n=1:size(cop_LR_time,1)
% %     plot3(cop_LR_values(n,1),cop_LR_values(n,2),cop_LR_time(n),'ok')
% %     pause(0.02)
% % end
%
%%
% for n=8:size(cop_LR_values,1)
%     cop_LR_values_F(n,1)=mean(cop_LR_values(n-7:n,1));
%     cop_LR_values_F(n,2)=mean(cop_LR_values(n-7:n,2));
% end
% 
% 
% figure
% hold on
% plot(cop_LR_values_F(10:end,2),cop_LR_values_F(10:end,1),'r')

%%

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

subplot(2,1,1)
plot(cop_LR_time,cop_LR_values(:,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',[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('X (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(cop_LR_time,cop_LR_values(:,2),'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('Y (m)')
xlabel('Time (s)')
% % 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')

%%
% for n=1:size(
humanoid_values_int=interp1(humanoid_time,humanoid_values,phantom_time);

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
plot(humanoid_time,humanoid_values(:,13+2),'r','LineWidth',2)
plot(humanoid_time,humanoid_values(:,13+3),'r','LineWidth',2)
plot(humanoid_time,humanoid_values(:,13+4),'b','LineWidth',2)
plot(humanoid_time,humanoid_values(:,13+5),'b','LineWidth',2)
plot(humanoid_time,humanoid_values(:,13+8),'g','LineWidth',2)
plot(humanoid_time,humanoid_values(:,13+9),'g','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 (º)')
xlabel('Time (s)')


h6=figure;
set(h6,'Name','humanoide_joints_w/_phantom_variations','Color',[1,1,1], 'Position' ,[100 100 980 630],'PaperPositionMode','auto','PaperUnits','centimeters',...
    'PaperSize',[23 19])
hold on
plot(phantom_values(:,3),humanoid_values_int(:,(13+2)),'r','LineWidth',2)
plot(phantom_values(:,3),humanoid_values_int(:,13+3),'r','LineWidth',2)
plot(phantom_values(:,3),humanoid_values_int(:,13+4),'b','LineWidth',2)
plot(phantom_values(:,3),humanoid_values_int(:,13+5),'b','LineWidth',2)
plot(phantom_values(:,3),humanoid_values_int(:,13+8),'g','LineWidth',2)
plot(phantom_values(:,3),humanoid_values_int(:,13+9),'g','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 (º)')
xlabel('Phantom position in Z (mm)')

%%


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

subplot(2,1,1)
plot(cop_LR_time,cop_LR_values(:,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',[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('COP X (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(force_time,force_values(:,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) max(force_time)]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

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


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

subplot(2,1,1)
plot(cop_LR_time,cop_LR_values(:,2),'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',[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('COP Y (m)')
xlabel('Time (s)')

subplot(2,1,2)
plot(force_time,force_values(:,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) max(force_time)]...,'YTickMode','manual'...,'YTickLabel',[0;200;400;600;800;1023],'YTick',[0;200;400;600;800;1023])
    )

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