label = 'lumbar_extension';

%s1 = read_motionFile('../RRA/Results_Eran.good_end/GIL01_slow3_RRA2_Pass3_states.sto');
%s2 = read_motionFile('../RRA/Results_Eran.bad_end/GIL01_slow3_RRA2_Pass3_states.sto');
%s3 = read_motionFile('../RRA/Results_Eran.worst_end/GIL01_slow3_RRA2_Pass3_states.sto');
%
%s1.data(:,2:end) = s1.data(:,2:end) * 180/pi;
%s2.data(:,2:end) = s2.data(:,2:end) * 180/pi;
%s3.data(:,2:end) = s3.data(:,2:end) * 180/pi;
%
%s1.data = s1.data(1:end-1,:);
%s1.nr = s1.nr - 1;
%
%plot_columns({s1 s2 s3}, 'lumbar_extension\>');
%
%ss1 = read_motionFile('../RRA/Results_Eran.good_end/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%ss2 = read_motionFile('../RRA/Results_Eran.bad_end/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%ss3 = read_motionFile('../RRA/Results_Eran.worst_end/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%
%plot_columns({ss1 ss2 ss3}, label);
%
%return;
%
%I = find_columns_by_label(s1.labels, label);
%I = I(1);
%
%u1 = diff(s1.data(:,I))./diff(s1.data(:,1));
%u2 = diff(s2.data(:,I))./diff(s2.data(:,1));
%u3 = diff(s3.data(:,I))./diff(s3.data(:,1));
%
%plot(s1.data(1:end-1,1), [ u1 u2 u3 ]);
%
%a1 = diff(u1);
%a2 = diff(u2);
%a3 = diff(u3);
%
%figure;
%plot(s1.data(1:end-2,1), [ a1 a2 a3 ]);
%
%return;

ik = read_motionFile('../IK/GIL01_slow3_ik.mot');
ikI = find_columns_by_label(ik.labels,label);

%original_q = read_motionFile('../RRA/Results_Pass3/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%original_u = read_motionFile('../RRA/Results_Pass3/GIL01_slow3_RRA2_Pass3_Kinematics_u.sto');
%original_dudt = read_motionFile('../RRA/Results_Pass3/GIL01_slow3_RRA2_Pass3_Kinematics_dudt.sto');
original_q = read_motionFile('../RRA/Results_Eran/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
original_u = read_motionFile('../RRA/Results_Eran/GIL01_slow3_RRA2_Pass3_Kinematics_u.sto');
original_dudt = read_motionFile('../RRA/Results_Eran/GIL01_slow3_RRA2_Pass3_Kinematics_dudt.sto');
%original_q = read_motionFile('../RRA/Results_Eran.test1/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%original_u = read_motionFile('../RRA/Results_Eran.test1/GIL01_slow3_RRA2_Pass3_Kinematics_u.sto');
%original_dudt = read_motionFile('../RRA/Results_Eran.test1/GIL01_slow3_RRA2_Pass3_Kinematics_dudt.sto');
%original_q = read_motionFile('../RRA/Results_Eran.baseline/GIL01_slow3_RRA2_Pass3_Kinematics_q.sto');
%original_u = read_motionFile('../RRA/Results_Eran.baseline/GIL01_slow3_RRA2_Pass3_Kinematics_u.sto');
%original_dudt = read_motionFile('../RRA/Results_Eran.baseline/GIL01_slow3_RRA2_Pass3_Kinematics_dudt.sto');
q_nosplinefit_q = read_motionFile('desiredKinematics_nosplinefit_positions.sto');
q_splinefit_q = read_motionFile('desiredKinematics_splinefit_positions.sto');
q_splinefit_u = read_motionFile('desiredKinematics_splinefit_velocities.sto');
q_splinefit_dudt = read_motionFile('desiredKinematics_splinefit_accelerations.sto');

I=find_columns_by_label(q_splinefit_q.labels,label);

T = q_splinefit_q.data(:,1);
Q0 = q_nosplinefit_q.data(:,I);
Q = q_splinefit_q.data(:,I);
U = q_splinefit_u.data(:,I);
DUDT = q_splinefit_dudt.data(:,I);

testU = diff(Q)./diff(T);
testDUDT = diff(U)./diff(T);

T2 = original_q.data(:,1);
Q2 = original_q.data(:,I) * pi/180;
U2 = original_u.data(:,I) * pi/180;
DUDT2 = original_dudt.data(:,I) * pi/180;
testU2 = diff(Q2)./diff(T2);

close all;
figure;
plot(ik.data(:,1), ik.data(:,ikI));
title('IK');
figure;
plot(T(1:end-1), [Q0(1:end-1) Q(1:end-1) testU], ':');
hold on;
plot(T2, Q2);
title('RRA2 Q');
figure;
plot(T2(1:end-1), [testU2 U2(1:end-1) DUDT2(1:end-1)]);
title('RRA2 U and numerical U');
figure;
plot(T(1:end-1), testDUDT);
figure;
plot(T, [Q U DUDT]);
hold off;
