%close all;

%q1=read_motionFile('test.sto');
%q1=read_motionFile('delaware3_ss_walking1_ik.mot');
%q1=read_motionFile('Results/unfiltered_positions.sto');
%q1=read_motionFile('Results/filtered.sto');
q1=read_motionFile('Results/delaware3_ss_walking1_ik_Kinematics_q.sto');
%q1=read_motionFile('Results/delaware3_ss_walking1_ik_rra_Kinematics_q.sto');
%q1=read_motionFile('Results/checkPositions.sto');
%q1=read_motionFile('ResultsRRA2/delaware3_Kinematics_q.sto');
%q2=read_motionFile('ResultsCMC/delaware3_Kinematics_q.sto');
%q2=read_motionFile('Results/unfiltered_positions.sto');
%q1.data(:,2:end)=q1.data(:,2:end)*180/pi;
%q2.data(:,2:end)=q2.data(:,2:end)*180/pi;

I=find_columns_by_label(q1.labels,{'tib_pat_r_r1','knee_angle_r'});

%plot(q1.data(:,1),q1.data(:,I),q2.data(:,1),q2.data(:,I));
%legend({q1.labels{I} q2.labels{I}},'Interpreter','none');

plot(q1.data(:,1),q1.data(:,I));
legend({q1.labels{I}},'Interpreter','none');

%q3=read_motionFile('../delaware3/Results/delaware3_ss_walking1_ik_rra_Kinematics_q.sto');
%q4=read_motionFile('../delaware3/ResultsCMC/delaware3_Kinematics_q.sto');
%hold on;
%plot(q3.data(:,1),q3.data(:,I),q4.data(:,1),q4.data(:,I));
%hold off;

knee_fcn_x=[-120 -114.59 -83.51 -30.16 1.6 10];
knee_fcn_y=[17.65 17.65 17.55 15.48 -2.12 -16.04];
X=-120:0.1:10;
Y=interp1(knee_fcn_x,knee_fcn_y,X,'spline');
I=find_columns_by_label(q1.labels,'knee_angle_r');
hold on;
plot(q1.data(:,1),interp1(X,Y,q1.data(:,I)),'x');
hold off;
