%% Getting the force cells data... and other stuff
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

% Values from accelerometers to get the offset for each one of them
accel_max_min_val(:,:,1) = [-266 252
                            -256 263
                            -255 264];

accel_max_min_val(:,:,2) = [-257 260
                            -255 265
                            -264 256];
                        
accel_max_min_val(:,:,3) = [-274 254
                            -253 265
                            -260 260];
                        
accel_max_min_val(:,:,4) = [-258 260
                            -258 268
                            -259 248];

accel_max_min_val(:,:,5) = [-264 253
                            -265 273
                            -253 250];

accel_max_min_val(:,:,6) = [-274 255
                            -258 265
                            -272 260];
                        
accel_max_min_val(:,:,7) = [-265 255
                            -255 265
                            -261 253];
                        
accel_max_min_val(:,:,8) = [-264 252
                            -248 255
                            -258 259];
                        
accel_max_min_val(:,:,9) = [-268 265
                            -255 271
                            -285 230];

% sensors's offsets determination
accel_offset = zeros(9,3);
for sensor_n = 1:size(accel_offset,1)
    accel_offset(sensor_n,1) = (accel_max_min_val(1,1,sensor_n)+accel_max_min_val(1,2,sensor_n))/2;
    accel_offset(sensor_n,2) = (accel_max_min_val(2,1,sensor_n)+accel_max_min_val(2,2,sensor_n))/2;
    accel_offset(sensor_n,3) = (accel_max_min_val(3,1,sensor_n)+accel_max_min_val(3,2,sensor_n))/2;
end


% subplot(1,2,1)
hold on
% plot(time_1,force_1(:,1),'b')
% plot(time_1,force_1(:,2),'r')
% plot(time_1,force_1(:,3),'g')
% plot(time_1,force_1(:,4),'k')
% subplot(1,2,2)
% 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*1.2,'y','LineWidth',4) % Gz_S6
plot(sensors_time(:,5),sensors_data(:,6,5)*convert_fact*1.2,'b','LineWidth',4) % Gz_S5

% plot(time_1,moving_average(force_1(:,1),200),'b')
% plot(time_1,moving_average(force_1(:,2),200),'r')
% plot(time_1,moving_average(force_1(:,3),200),'g')
% plot(time_1,moving_average(force_1(:,4),200),'k')
% legend('servo 12 - angle','servo 12 - speed','gyro 6 - Z','cell 1','cell 2','cell 3','cell4')
legend('servo 12 - angle','servo 13 - angle','gyro 6 - Z','gyro 5 - Z')

figure
hold on
plot(humanoid_time,humanoid_state(:,4),'r','LineWidth',3)

plot(humanoid_time(1:(end-1)),diff(humanoid_state(:,4)),'k','LineWidth',3) %servo 22

plot(sensors_time(:,8),-sensors_data(:,6,8)*convert_fact*1.2,'y','LineWidth',4) % Gz_S8
plot(sensors_time(:,7),-sensors_data(:,6,7)*convert_fact*1.2,'b','LineWidth',4) % Gz_S7
legend('servo 22 - angle','servo 22 - speed','gyro 8 - Z','gyro 7 - Z')

% plot(sensors_time(:,6),sensors_data(:,5,6)*convert_fact,'c','LineWidth',4)
% plot(sensors_time(:,6),sensors_data(:,4,6)*convert_fact,'r','LineWidth',4)
% subplot(1,3,3)
% hold on
% plot(time_2,force_2(:,1),'b')
% plot(time_2,force_2(:,2),'r')
% plot(time_2,force_2(:,3),'g')
% plot(time_2,force_2(:,4),'k')

%% 

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;

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) - humanoid_state(1,14))*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
init_time = data(1,3);
% 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

% Values from accelerometers to get the offset for each one of them
accel_max_min_val(:,:,1) = [-266 252
                            -256 263
                            -255 264];

accel_max_min_val(:,:,2) = [-257 260
                            -255 265
                            -264 256];
                        
accel_max_min_val(:,:,3) = [-274 254
                            -253 265
                            -260 260];
                        
accel_max_min_val(:,:,4) = [-258 260
                            -258 268
                            -259 248];

accel_max_min_val(:,:,5) = [-264 253
                            -265 273
                            -253 250];

accel_max_min_val(:,:,6) = [-274 255
                            -258 265
                            -272 260];
                        
accel_max_min_val(:,:,7) = [-265 255
                            -255 265
                            -261 253];
                        
accel_max_min_val(:,:,8) = [-264 252
                            -248 255
                            -258 259];
                        
accel_max_min_val(:,:,9) = [-268 265
                            -255 271
                            -285 230];

% sensors's offsets determination
accel_offset = zeros(9,3);
for sensor_n = 1:size(accel_offset,1)
    accel_offset(sensor_n,1) = (accel_max_min_val(1,1,sensor_n)+accel_max_min_val(1,2,sensor_n))/2;
    accel_offset(sensor_n,2) = (accel_max_min_val(2,1,sensor_n)+accel_max_min_val(2,2,sensor_n))/2;
    accel_offset(sensor_n,3) = (accel_max_min_val(3,1,sensor_n)+accel_max_min_val(3,2,sensor_n))/2;
end

subplot(2,1,1)
hold on
plot(time_1,moving_average(force_1(:,1),150),'r')
plot(time_1,moving_average(force_1(:,2),150),'g')
plot(time_1,moving_average(force_1(:,3),150),'b')
plot(time_1,moving_average(force_1(:,4),150),'k')

plot(humanoid_time,humanoid_state(:,3),'k','LineWidth',3)

subplot(2,1,2)
hold on
% [ax y1 y2] = plotyy(humanoid_time(1:(end-1)),diff(humanoid_state(:,3)),sensors_time(:,5),-sensors_data(:,6,5)*convert_fact)
% set(y1,'Color',[1 1 0],'LineWidth',3)
% set(y2,'Color',[0 1 0],'LineWidth',3)
plot(humanoid_time(1:(end-1)),diff(humanoid_state(:,3)),'Color',[1 1 0],'LineWidth',3)
plot(sensors_time(:,5),-sensors_data(:,6,5)*convert_fact,'r','LineWidth',3)

legend('13 joint speed','sensor 5 - speed')


% getting the MAGNITUDE values from Accelerometers.
magnitude = zeros(size(sensors_data,1),size(sensors_data,3));
for m=1:size(sensors_data,3)
    for n=1:size(sensors_data,1)
        magnitude(n,m) = sqrt((sensors_data(n,1,m)+accel_offset(m,1))^2+(sensors_data(n,2,m)+accel_offset(m,2))^2+(sensors_data(n,3,m)+accel_offset(m,3))^2);
    end
end
div_magnitude = magnitude - 255;


% ACCELEROMETERS


figure
title('anca - acelerometros - eixo X global')
hold on
plot(sensors_time(:,1),moving_average(sensors_data(:,1,1)-accel_offset(1,1),10),'r','LineWidth',3)
plot(sensors_time(:,2),moving_average(sensors_data(:,1,2)-accel_offset(2,1),10),'k','LineWidth',3)
plot(sensors_time(:,3),moving_average(-sensors_data(:,1,3)+accel_offset(3,1),10),'b','LineWidth',3)
plot(sensors_time(:,4),moving_average(-sensors_data(:,1,4)+accel_offset(4,1),10),'g','LineWidth',3)
legend('sensor 1 ADC','sensor 2 ADC','sensor 3 ADC','sensor 4 ADC')

figure
title('anca -acelerometros - eixo Y global')
hold on
plot(sensors_time(:,1),moving_average(sensors_data(:,3,1)-accel_offset(1,3),10),'r','LineWidth',3)
plot(sensors_time(:,2),moving_average(sensors_data(:,2,2)-accel_offset(2,2),10),'k','LineWidth',3)
plot(sensors_time(:,3),moving_average(-sensors_data(:,2,3)+accel_offset(3,2),10),'b','LineWidth',3)
plot(sensors_time(:,4),moving_average(-sensors_data(:,3,4)+accel_offset(4,3),10),'g','LineWidth',3)
legend('sensor 1 ADC','sensor 2 ADC','sensor 3 ADC','sensor 4 ADC')

figure
title('anca - acelerometros - eixo Z global')
hold on
plot(sensors_time(:,1),moving_average(-sensors_data(:,2,1)-accel_offset(1,2),10),'r','LineWidth',3)
plot(sensors_time(:,2),moving_average(sensors_data(:,3,2)-accel_offset(2,3),10),'k','LineWidth',3)
plot(sensors_time(:,3),moving_average(sensors_data(:,3,3)+accel_offset(3,3),10),'b','LineWidth',3)
plot(sensors_time(:,4),moving_average(-sensors_data(:,2,4)+accel_offset(4,2),10),'g','LineWidth',3)
legend('sensor 1 ADC','sensor 2 ADC','sensor 3 ADC','sensor 4 ADC')

figure
title('anca - giroscópios - eixo X global')
hold on
plot(sensors_time(:,1),sensors_data(:,4,1)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,2),sensors_data(:,4,2)*convert_fact,'k','LineWidth',3)
plot(sensors_time(:,3),-sensors_data(:,4,3)*convert_fact,'b','LineWidth',3)
plot(sensors_time(:,4),-sensors_data(:,4,4)*convert_fact,'g','LineWidth',3)
legend('sensor 1 º/s','sensor 2 º/s','sensor 3 º/s','sensor 4 º/s')

figure
title('anca - giroscópios - eixo Y global')
hold on
plot(sensors_time(:,1),sensors_data(:,6,1)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,2),sensors_data(:,5,2)*convert_fact,'k','LineWidth',3)
plot(sensors_time(:,3),-sensors_data(:,5,3)*convert_fact,'b','LineWidth',3)
plot(sensors_time(:,4),-sensors_data(:,6,4)*convert_fact,'g','LineWidth',3)
legend('sensor 1 º/s','sensor 2 º/s','sensor 3 º/s','sensor 4 º/s')

figure
title('anca - giroscópios - eixo Z global')
hold on
plot(sensors_time(:,1),-sensors_data(:,5,1)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,2),sensors_data(:,6,2)*convert_fact,'k','LineWidth',3)
plot(sensors_time(:,3),sensors_data(:,6,3)*convert_fact,'b','LineWidth',3)
plot(sensors_time(:,4),-sensors_data(:,5,4)*convert_fact,'g','LineWidth',3)
legend('sensor 1 º/s','sensor 2 º/s','sensor 3 º/s','sensor 4 º/s')






figure
title('perna direita- acelerómetros - eixo X global')
hold on
plot(sensors_time(:,5),moving_average(sensors_data(:,2,5)-accel_offset(5,2),10),'g','LineWidth',3)
plot(sensors_time(:,6),moving_average(sensors_data(:,2,6)-accel_offset(6,2),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')

figure
title('perna direita- acelerómetros - eixo Y global')
hold on
plot(sensors_time(:,5),moving_average(sensors_data(:,3,5)-accel_offset(5,3),10),'g','LineWidth',3)
plot(sensors_time(:,6),moving_average(sensors_data(:,3,6)-accel_offset(6,3),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')

figure
title('perna direita- acelerómetros - eixo Z global')
hold on
plot(sensors_time(:,5),moving_average(sensors_data(:,1,5)-accel_offset(5,1),10),'g','LineWidth',3)
plot(sensors_time(:,6),moving_average(sensors_data(:,1,6)-accel_offset(6,1),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')

figure
title('perna esquerda- acelerómetros - eixo X global')
hold on
plot(sensors_time(:,7),-moving_average(sensors_data(:,2,7)-accel_offset(7,2),10),'g','LineWidth',3)
plot(sensors_time(:,8),-moving_average(sensors_data(:,2,8)-accel_offset(8,2),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')

figure
title('perna esquerda- acelerómetros - eixo Y global')
hold on
plot(sensors_time(:,7),-moving_average(sensors_data(:,3,7)-accel_offset(7,3),10),'g','LineWidth',3)
plot(sensors_time(:,8),-moving_average(sensors_data(:,3,8)-accel_offset(8,3),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')

figure
title('perna esquerda- acelerómetros - eixo Z global')
hold on
plot(sensors_time(:,7),moving_average(sensors_data(:,2,7)-accel_offset(7,2),10),'g','LineWidth',3)
plot(sensors_time(:,8),moving_average(sensors_data(:,2,8)-accel_offset(8,2),10),'b','LineWidth',3)
legend('sensor 5 ADC','sensor 6 ADC')


figure
title('perna direita- giroscópios - eixo X global')
hold on
plot(sensors_time(:,5),sensors_data(:,5,5)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,6),sensors_data(:,5,6)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')

figure
title('perna direita- giroscópios - eixo Y global')
hold on
plot(sensors_time(:,5),sensors_data(:,6,5)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,6),sensors_data(:,6,6)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')

figure
title('perna direita- giroscópios - eixo Z global')
hold on
plot(sensors_time(:,5),sensors_data(:,4,5)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,6),sensors_data(:,4,6)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')

figure
title('perna esquerda- giroscópios - eixo X global')
hold on
plot(sensors_time(:,7),-sensors_data(:,5,7)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,8),-sensors_data(:,5,8)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')

figure
title('perna esquerda- giroscópios - eixo Y global')
hold on
plot(sensors_time(:,7),-sensors_data(:,6,7)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,8),-sensors_data(:,6,8)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')

figure
title('perna esquerda- giroscópios - eixo Z global')
hold on
plot(sensors_time(:,7),sensors_data(:,4,7)*convert_fact,'r','LineWidth',3)
plot(sensors_time(:,8),sensors_data(:,4,8)*convert_fact,'k','LineWidth',3)
legend('sensor 5 º/s','sensor 6 º/s')
