clear all
close all
clc

SimParam
%%%%%%%%%%%%%%%%%%%%%%%Script%%%%%%%%%%%%%%%%%%%%%%%Data%%%%%%%%%%%%%%%%%%%
arduino_1_value=load('Data\arduino_1_values_report');
arduino_2_value=load('Data\arduino_2_values_report');
cop_value_l=load('Data\cop_left_report');
cop_value_r=load('Data\cop_right_report');
cop_value_l_r=load('Data\cop_left_right_report');
force_value=load('Data\force_report_name_');
humanoid_value=load('Data\humanoid_fan_report');
phantom_state_value=load('Data\phantom_state_report');
phantom_filter_value=load('Data\phantom_filter_report');

save Data.mat arduino_1_value arduino_2_value cop_value_l cop_value_r cop_value_l_r force_value humanoid_value phantom_state_value phantom_filter_value;
load('Data.mat');

posFanuc=load('positions.txt');
Pos=posFanuc(:,5);
temp_Pos=posFanuc(:,1)-posFanuc(1,1);

%for i=1:max(size(temp_Pos))
   
%    temp_Pos(i)=temp_Pos(i)-1.5;
%end
%%%%%%%%%%%%%%%%%%%%%%%Pos_fanuc%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Pos=posFanuc(:,4);

%%%%%%%%%%%%%%%%%%arduino1%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_Ard_1=arduino_1_value(:,2)-arduino_1_value(1,2);
ard_1_1=arduino_1_value(:,3);
ard_1_2=arduino_1_value(:,4);
ard_1_3=arduino_1_value(:,5);
ard_1_4=arduino_1_value(:,6);

%%%%%%%%%%%%%%%%%%arduino2%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_Ard_2=arduino_2_value(:,2)-arduino_2_value(1,2);
ard_2_1=arduino_2_value(:,3);
ard_2_2=arduino_2_value(:,4);
ard_2_3=arduino_2_value(:,5);
ard_2_4=arduino_2_value(:,6);

%%%%%%%%%%%%%%%%%%%%%%%%%%%Cop_L%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_cop_l=cop_value_l(:,2)-cop_value_l(1,2);
x_l=cop_value_l(:,3);
y_l=cop_value_l(:,4);

%%%%%%%%%%%%%%%%%%%%%%%%%%Cop_L_r%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_cop_l_r=cop_value_l_r(:,2)-cop_value_l_r(1,2);
x_l_r=cop_value_l_r(:,3);
y_l_r=cop_value_l_r(:,4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%Cop_r%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_cop_r=cop_value_r(:,2)-cop_value_r(1,2);
x_r=cop_value_r(:,3);
y_r=cop_value_r(:,4);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%force%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
temp_force=force_value(:,2)-force_value(1,2);
force_x=force_value(:,3);
force_y=force_value(:,4);
force_z=force_value(:,5);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%humanoid%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 temp_hum=humanoid_value(:,2)-humanoid_value(1,2);
% %speed_now
 speed_now_1=humanoid_value(:,3);
 speed_now_2=humanoid_value(:,4);
 speed_now_3=humanoid_value(:,5);
 speed_now_4=humanoid_value(:,6);
 speed_now_5=humanoid_value(:,7);
 speed_now_6=humanoid_value(:,8);
 speed_now_7=humanoid_value(:,9);
 speed_now_8=humanoid_value(:,10);
 speed_now_9=humanoid_value(:,11);
 speed_now_10=humanoid_value(:,12);
 speed_now_11=humanoid_value(:,13);
 speed_now_12=humanoid_value(:,14);
% 
%%speed_wanted
 speed_wanted_1=humanoid_value(:,15);
 speed_wanted_2=humanoid_value(:,16);
 speed_wanted_3=humanoid_value(:,17);
 speed_wanted_4=humanoid_value(:,18);
 speed_wanted_5=humanoid_value(:,19);
 speed_wanted_6=humanoid_value(:,20);
 speed_wanted_7=humanoid_value(:,21);
 speed_wanted_8=humanoid_value(:,22);
 speed_wanted_9=humanoid_value(:,23);
 speed_wanted_10=humanoid_value(:,24);
 speed_wanted_11=humanoid_value(:,25);
 speed_wanted_12=humanoid_value(:,26);
% 
% %joint_now
 joint_now_1=humanoid_value(:,27);
 joint_now_2=humanoid_value(:,28);
 joint_now_3=humanoid_value(:,29);
 joint_now_4=humanoid_value(:,30);
 joint_now_5=humanoid_value(:,31);
 joint_now_6=humanoid_value(:,32);
 joint_now_7=humanoid_value(:,33);
 joint_now_8=humanoid_value(:,34);
 joint_now_9=humanoid_value(:,35);
 joint_now_10=humanoid_value(:,36);
 joint_now_11=humanoid_value(:,37);
 joint_now_12=humanoid_value(:,38);
% 
% %joint_wanted
 joint_wanted_1=humanoid_value(:,39);
 joint_wanted_2=humanoid_value(:,40);
 joint_wanted_3=humanoid_value(:,41);
 joint_wanted_4=humanoid_value(:,42);
 joint_wanted_5=humanoid_value(:,43);
 joint_wanted_6=humanoid_value(:,44);
 joint_wanted_7=humanoid_value(:,45);
 joint_wanted_8=humanoid_value(:,46);
 joint_wanted_9=humanoid_value(:,47);
 joint_wanted_10=humanoid_value(:,47);
 joint_wanted_11=humanoid_value(:,48);
 joint_wanted_12=humanoid_value(:,49);

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%filtro%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 temp_filt=phantom_filter_value(:,2)-phantom_filter_value(1,2);
 pos_filter_x=phantom_filter_value(:,3);
 pos_filter_y=phantom_filter_value(:,4);
 pos_filter_z=phantom_filter_value(:,5);
 
 rot_filter_x=phantom_filter_value(:,6);
 rot_filter_y=phantom_filter_value(:,7);
 rot_filter_z=phantom_filter_value(:,8);
% 
 button_filter_x=phantom_filter_value(:,9);
 button_filter_y=phantom_filter_value(:,10);

 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%phantom%%%%%control%%%%%%%%%%%%%%%%%%%%
 temp_control=phantom_state_value(:,2)-phantom_state_value(1,2);
 pos_state_x=phantom_state_value(:,3);
 pos_state_y=phantom_state_value(:,4);
 pos_state_z=phantom_state_value(:,5);
% 
 rot_state_x=phantom_state_value(:,6);
 rot_state_y=phantom_state_value(:,7);
 rot_state_z=phantom_state_value(:,8);
% 
 joints_state_x=phantom_state_value(:,9);
 joints_state_y=phantom_state_value(:,10);
 joints_state_z=phantom_state_value(:,11);
% 
 force_state_x=phantom_state_value(:,12);
 force_state_y=phantom_state_value(:,13);
 force_state_z=phantom_state_value(:,14);
% 
 button_state_x=phantom_state_value(:,15);
 button_state_y=phantom_state_value(:,16);
 
 home_state_x=phantom_state_value(:,17);
% 
 hom_pos_state_x=phantom_state_value(:,18);
 home_pos_state_y=phantom_state_value(:,19);
 home_pos_state_z=phantom_state_value(:,20);
%%%%%%%%%%%%%%%%%%%%Calculo das Métricas%%%%%%%%%%%%%%%%%%

%Metrica 1

%Metrica 1
M1x=0;
mx=mean(x_l_r);
for i=1:max(size(x_l_r))
    M1xT=(x_l_r(i)-mx)^2/max(size(x_l_r));
    M1x=M1x+M1xT;
end
M1x=sqrt(M1x)*100;


M1y=0;
my=mean(y_l_r);
for i=1:max(size(y_l_r))
    M1yT=(y_l_r(i)-my)^2/max(size(y_l_r));
    M1y=M1y+M1yT;
end
M1y=sqrt(M1y)*100;
M1T=M1y+M1x;
M1y=num2str(M1y);
M1x=num2str(M1x);
M1T=num2str(M1T);  % Metrica Total

sy=std(y_l_r)
%%%Métrica 2; Comparação ao ponto inicial
M2x=0;
for i=1:max(size(x_l_r))
    M2x=M2x+abs((x_l_r(i)-x_l_r(1)));
end
M2x=(M2x/max(size(x_l_r)))*100;


M2y=0;
for i=1:max(size(y_l_r))
    M2y=M2y+abs((y_l_r(i)-y_l_r(1)));
end
M2y=(M2y/max(size(y_l_r)))*100;
M2T=M2y+M2x;
M2y=num2str(M2y);
M2x=num2str(M2x);
M2T=num2str(M2T);
A1=max(x_l_r)-min(x_l_r);
A2=max(y_l_r)-min(y_l_r);
A=A1*A2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xD='x';yD='y';tD='total';M_1='M1';M_2='M2';aD='area';
DataMat={' ',xD,yD,tD,aD; M_1, M1x,M1y,M1T,A;M_2, M2x,M2y,M2T,A;}


% %%%%%%%%%%%%%%%%%%%%%%%%%Cinematica directa%%%%%%%%%%%%%%%%%%
% for i=1:max(size(joint_now_1))
% P_ref_r = [0 0 0]; 
% qi_r = [joint_now_1(i) joint_now_3(i) joint_now_5(i) 0 joint_now_7(i) joint_now_9(i)]';   qi_l = [0 0 0 0 0 0]'; 
% [P_r(:,i),P_hip] = DirKin6dof_stance( P_ref_r,qi_r,L,+1 );
% 
% end
% 
% for i=1:max(size(P_r))
% 
% P1(1,i)=P_r(1,i);
% P1(2,i)=P_r(2,i);
% P1(3,i)=P_r(3,i);
% 
% 
% P2(1,i)=P_r(4,i);
% P2(2,i)=P_r(5,i);
% P2(3,i)=P_r(6,i);
% 
% 
% P3(1,i)=P_r(7,i);
% P3(2,i)=P_r(8,i);
% P3(3,i)=P_r(9,i);
% 
% 
% P4(1,i)=P_r(10,i);
% P4(2,i)=P_r(11,i);
% P4(3,i)=P_r(12,i);
% 
% 
% P5(1,i)=P_r(13,i);
% P5(2,i)=P_r(14,i);
% P5(3,i)=P_r(15,i);
% end


%%PLOAT Directe%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% figure (1)
% t=linspace(0,45,max(size(P1)))
% figure (2)
% plot(t,P1(2,:))
% hold on
% plot(t,P1(3,:),'r');
% hold on
% plot(t,P1(1,:),'g');
% legend('X','Y','Z')
% hold off
% 
% figure (2)
% plot(t,P2(2,:))
% hold on
% plot(t,P2(3,:),'r');
% hold on
% plot(t,P2(1,:),'g');
% legend('X','Y','Z')
% hold off
% %figure (3)
% %plot3(P2(1,:),P2(2,:),P2(3,:))
% figure (3)
% plot(t,P3(2,:))
% hold on
% plot(t,P3(3,:),'r');
% hold on
% plot(t,P3(1,:),'g');
% legend('X','Y','Z')
% hold off
% %figure (4)
% %plot3(P3(1,:),P3(2,:),P3(3,:))
% figure (4)
% plot(t,P4(2,:))
% hold on
% plot(t,P4(3,:),'r');
% hold on
% plot(t,P4(1,:),'g');
% legend('X','Y','Z')
% hold off
% %figure (5)
% %plot3(P4(1,:),P4(2,:),P4(3,:))
% figure (5)
% plot(t,P5(2,:))
% hold on
% plot(t,P5(3,:),'r');
% hold on
% plot(t,P5(1,:),'g');
% legend('X','Y','Z')
% hold off

%%%%%%%%%%%%%%%%Plot%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

figure (1)
plot(y_l_r,x_l_r,'LineWidth',1);
xlabel('y(m)','FontSize',16)
ylabel('x(m)','FontSize',16)
title('COP\_Value','FontSize', 16);
rectangle('Position',[-0.023-0.07 -0.046 0.023*2 0.044+0.046 ],'curvature',[0.2 0.1],'LineWidth',2,'LineStyle','-')
hold on
rectangle('Position',[-0.023-0.07+0.14 -0.046 0.023*2 0.044+0.046 ],'curvature',[0.2 0.1],'LineWidth',2,'LineStyle','-')
hold on
text(0-0.07,-0.04,'Right','HorizontalAlignment','center','FontSize',16)
text(0.14-0.07,-0.04,'Left','HorizontalAlignment','center','FontSize',16)
 text(0-0.07,0.038,'Front','HorizontalAlignment','center','FontSize',16)
text(0.14-0.07,0.038,'Front','HorizontalAlignment','center','FontSize',16)
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%COPx COPy%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure (2)
subplot(211)
plot(temp_cop_l_r,x_l_r,'r','LineWidth',2)
xlabel('time(s)','FontSize',16)
ylabel('X(m)','FontSize',16)
axis([ 0 30 -0.045 0.045])
subplot(212)
plot(temp_cop_l_r,y_l_r,'lineWidth',2)
xlabel('time(s)','FontSize',16)
ylabel('Y(m)','FontSize',14)
axis([ 0 30 -0.045 0.045])

%%%%%%%%%%%%%%%Plot Force%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure (3)
plot(temp_force,force_x,'LineWidth',2)
xlabel('Time(s)','FontSize',14)
ylabel('Force(N)','FontSize',14)
% hold on
% plot(temp_force,force_y,'r','LineWidth',2)
hold on
plot(temp_force,force_z,'g','LineWidth',2)
legend('Fx','Fz')
axis([0 30 -2.5 2.5])
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%Phantom_control%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%   figure (9)
%   subplot(121)
%  plot(temp_control,pos_state_x,'LineWidth',2)
%  xlabel('time(s)','FontSize',16)
%  ylabel('Phantom´s (mm)','FontSize',16)
%   title('Phantom Data','FontSize',16)
%  hold on
% %  plot(temp_control,pos_state_y,'r','LineWidth',2)
% %  hold on
%  plot(temp_control,pos_state_z,'g','LineWidth',2)
%  hold on
%  plot(temp_Pos,Pos,'--k','LineWidth',2)
%  legend('y','x','Fanuc axis')
% axis([0 30 -35 35])
%  hold off
%  subplot(122)

load('matlab.mat')

posx=pos2_filter_x;
posz=pos2_filter_z;

 %%%%%%%%%%%%%%%%%%%%%%%%%Phantom_filter%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
 t=linspace(0,30,max(size(pos_filter_x)));
 tp=linspace(0,30,max(size(posx)));
 figure
 subplot(121)
 plot(t,pos_filter_x,'LineWidth',2)
 hold on
 plot(tp,posx,'g','LineWidth',2)
 hold on
 plot(temp_Pos,Pos,'--k','LineWidth',2)
 xlabel('time(s)','FontSize',16)
 ylabel('Phantom´s (mm)','FontSize',16)
  title('Phantom Filtered Data (x axis)','FontSize',16)
  legend('x (Force feedback)', 'x ','Fanuc axis')
  axis([0 30 -45 45])
%  hold on
%  plot(t,pos_filter_y,'r','LineWidth',2)
 subplot(122)
 title('Phantom Filtered Data (z axis)','FontSize',16)
 plot(t,pos_filter_z,'LineWidth',2)
  hold on
  plot(tp,posz,'g','LineWidth',2)
  hold on
 plot(temp_Pos,Pos,'--k','LineWidth',2)
 xlabel('time(s)','FontSize',16)
 ylabel('Phantom´s (mm)','FontSize',16)
 legend('z (force feedback)', 'z','Fanuc axis')
 axis([0 30 -45 45])
 hold off
 
 %%%%%%%%%%%%%%%%%%%%%%%%%humanoid%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

 load('matlab1.mat')
 
 figure (5)
 subplot(121)
 plot(temp_hum,joint_now_1,'LineWidth',2)
 hold on
 plot(temp,joint1,'g','LineWidth',2)
 xlabel('Time(s)','FontSize',16)
 ylabel('Joint Angle º','FontSize',16)
 hold on
 plot(temp_Pos,Pos,'--k','LineWidth',2)
 hold on
  axis([0 30 -15 15])
 legend('Joint\_1','Joint\_1 (Force feedback)','Fanuc axis')
%  plot(temp_hum,joint_now_2,'r','LineWidth',2)
%  hold on
 subplot(122)
  plot(temp_hum,joint_now_3,'LineWidth',2)
 hold on
plot(temp,joint2,'g','LineWidth',2)
 hold on
 plot(temp_Pos,Pos,'--k','LineWidth',2)
 legend('Joint\_2','Joint\_2 (Force feedback)','Fanuc axis')
 hold on
 xlabel('Time(s)','FontSize',16)
 ylabel('Joint Angle º','FontSize',16)
 axis([0 30 -15 15])
 hold off
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ARduino%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure (6)
plot(temp_Ard_1,ard_1_1,'LineWidth',2);
xlabel('Time(s)','FontSize',14)
ylabel('Force','FontSize',14)
hold on
plot(temp_Ard_1,ard_1_2,'r','LineWidth',2);
hold on
plot(temp_Ard_1,ard_1_3,'g','LineWidth',2);
hold on
plot(temp_Ard_1,ard_1_4,'k','LineWidth',2);
legend('LoadCell_1','LoadCell_2','LoadCell_3','LoadCell_4')
hold off


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%ARduino%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure (7)
plot(temp_Ard_2,ard_2_1,'LineWidth',2);
xlabel('Time(s)','FontSize',14)
ylabel('Force','FontSize',14)
hold on
plot(temp_Ard_2,ard_2_2,'r','LineWidth',2);
hold on
plot(temp_Ard_2,ard_2_3,'g','LineWidth',2);
hold on
plot(temp_Ard_2,ard_2_4,'k','LineWidth',2);
legend('LoadCell_1','LoadCell_2','LoadCell_3','LoadCell_4')
hold off