clear all , close all, clc
% the 4th column is the empty one!!
force_cells_1 = 'force_cells_1_ZZ.csv';
force_cells_2 = 'force_cells_2_ZZ.csv';
humanoid_state_zz = 'movezz_humanoid_state.csv';
zz_angular_displacement = 'movezz_angular_displacement.csv';
convert_fact = 0.061035156;
% calculado por mim, 0.061035156
% por outro lado, 0.069565
importfile(force_cells_1);
time_1 = (data(:,1)-data(1,1))*1e-9;
force_1(:,1) = data(:,5);
force_1(:,2) = data(:,6);
force_1(:,3) = data(:,7);
force_1(:,4) = data(:,8);

init_time = data(1,1);

importfile(force_cells_2);
time_2 = (data(:,1)-data(1,1))*1e-9;
force_2(:,1) = data(:,5);
force_2(:,2) = data(:,6);
force_2(:,3) = data(:,7);
force_2(:,4) = data(:,8);

importfile(humanoid_state_zz);
humanoid_state(:,1) = data(:,19); % servo 11
humanoid_state(:,2) = data(:,20); % servo 21
humanoid_state(:,3) = data(:,21); % servo 12
humanoid_state(:,4) = data(:,22); % servo 22
humanoid_state(:,5) = data(:,23); % servo 13
humanoid_state(:,6) = data(:,24); % servo 23
humanoid_state(:,7) = data(:,25); % servo 15
humanoid_state(:,8) = data(:,26); % servo 25
humanoid_state(:,9) = data(:,27); % servo 16
humanoid_state(:,10) = data(:,28); % servo 26
humanoid_state(:,11) = data(:,29); % servo 31
humanoid_state(:,12) = data(:,30); % servo 32
humanoid_state(:,13) = data(:,31); % servo (rotacao_tronco)
humanoid_state(:,14) = data(:,3); % TimeStamp
humanoid_time = (humanoid_state(:,14) - init_time)*1e-9;
% anulamento de campos com erro de comunicacao
x = find(humanoid_state==65530);
for q = 1:size(x)
    humanoid_state(x(q))=humanoid_state(x(q)-1);
end

% obtencao dos dados dos sensors
% Ax,Ay,Az,Gx,Gy,Gz,displacementX,displacementY,displacementZ,Mx,My,Mz,TimeStamp
importfile(zz_angular_displacement);
total_number = data(1,21);
for q = 1:total_number
    sensors_data(:,1,q)=data(:,8+((q-1)*18)); % S_Ax
    sensors_data(:,2,q)=data(:,9+((q-1)*18)); % S_Ay
    sensors_data(:,3,q)=data(:,10+((q-1)*18)); % S_Az
    sensors_data(:,4,q)=data(:,11+((q-1)*18)); % S_Gx
    sensors_data(:,5,q)=data(:,12+((q-1)*18)); % S_Gy
    sensors_data(:,6,q)=data(:,13+((q-1)*18)); % S_Gz
    sensors_data(:,7,q)=data(:,14+((q-1)*18)); % S_displacement_x
    sensors_data(:,8,q)=data(:,15+((q-1)*18)); % S_displacement_y
    sensors_data(:,9,q)=data(:,16+((q-1)*18)); % S_displacement_z
    sensors_data(:,10,q)=data(:,17+((q-1)*18)); % Mx
    sensors_data(:,11,q)=data(:,18+((q-1)*18)); % My
    sensors_data(:,12,q)=data(:,19+((q-1)*18)); % Mz
    sensors_data(:,13,q)=data(:,6+((q-1)*18)); % TimeStamp
end
    init_time_sensors = sensors_data(1,13,:);
for q = 1:total_number
    sensors_time(:,q) = (sensors_data(:,13,q) - init_time)*1e-9;
end


hold on
plot(humanoid_time,humanoid_state(:,3),'r','LineWidth',3)
plot(humanoid_time(1:(end-1)),diff(humanoid_state(:,3)),'k','LineWidth',3) %servo 12

plot(sensors_time(:,6),sensors_data(:,6,6)*convert_fact,'y','LineWidth',3) % Gz_S6
plot(sensors_time(:,5),sensors_data(:,6,5)*convert_fact,'b','LineWidth',3) % Gz_S5

legend('servo 12 - angle º','servo 12 - speed º/s','gyro 6 - Z º/s','gyro 5 - Z º/s')