function yout = plot_cop(inputfile,path1)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% plot_cop.m produces plots of the center of pressure position with
% respect to the foot markers for gait studies using the
% Harvard/Bertec setup.  This allows one to check that the ground
% reaction forces are being input correctly to SIMM.
%
% Inputs:
% inputfile - input file name (without .mat or .mot)
% path1     - subject initials and date
%
% Outputs:
% None; just produces diagnostic plots. 
%
% Jared Markowitz
% Summer 2011
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

addpath('~/Dropbox/jared/processMOT');

path1 = ['~/Documents/MATLAB/data/' path1 '/raw/'];

inmot = [path1 'edited_mot/' inputfile '.mot'];

[labels1,x1,y1] = readColData(inmot,'time');

status = fclose('all');

ncol = size(y1,2)+1;

A1 = cellstr(labels1);

vz1 = y1(:,find(strcmp(A1,'forceplate1_vz')>0)-1);
vz2 = y1(:,find(strcmp(A1,'forceplate2_vz')>0)-1);

lheelx = y1(:,find(strcmp(A1,'L.HEEL_px')>0)-1);
lheely = y1(:,find(strcmp(A1,'L.HEEL_py')>0)-1);
lheelz = y1(:,find(strcmp(A1,'L.HEEL_pz')>0)-1);
ltoex  = y1(:,find(strcmp(A1,'L.TOE_px')>0)-1);
ltoey  = y1(:,find(strcmp(A1,'L.TOE_py')>0)-1);
ltoez  = y1(:,find(strcmp(A1,'L.TOE_pz')>0)-1);
llatx  = y1(:,find(strcmp(A1,'L.LATFOOT_px')>0)-1);
llaty  = y1(:,find(strcmp(A1,'L.LATFOOT_py')>0)-1);
llatz  = y1(:,find(strcmp(A1,'L.LATFOOT_pz')>0)-1);
lmedx  = y1(:,find(strcmp(A1,'L.MEDFOOT_px')>0)-1);
lmedy  = y1(:,find(strcmp(A1,'L.MEDFOOT_py')>0)-1);
lmedz  = y1(:,find(strcmp(A1,'L.MEDFOOT_pz')>0)-1);

rheelx = y1(:,find(strcmp(A1,'R.HEEL_px')>0)-1);
rheely = y1(:,find(strcmp(A1,'R.HEEL_py')>0)-1);
rheelz = y1(:,find(strcmp(A1,'R.HEEL_pz')>0)-1);
rtoex  = y1(:,find(strcmp(A1,'R.TOE_px')>0)-1);
rtoey  = y1(:,find(strcmp(A1,'R.TOE_py')>0)-1);
rtoez  = y1(:,find(strcmp(A1,'R.TOE_pz')>0)-1);
rlatx  = y1(:,find(strcmp(A1,'R.LATFOOT_px')>0)-1);
rlaty  = y1(:,find(strcmp(A1,'R.LATFOOT_py')>0)-1);
rlatz  = y1(:,find(strcmp(A1,'R.LATFOOT_pz')>0)-1);
rmedx  = y1(:,find(strcmp(A1,'R.MEDFOOT_px')>0)-1);
rmedy  = y1(:,find(strcmp(A1,'R.MEDFOOT_py')>0)-1);
rmedz  = y1(:,find(strcmp(A1,'R.MEDFOOT_pz')>0)-1);

px1 = y1(:,find(strcmp(A1,'forceplate1_px')>0)-1);
py1 = y1(:,find(strcmp(A1,'forceplate1_py')>0)-1);
px2 = y1(:,find(strcmp(A1,'forceplate2_px')>0)-1);
py2 = y1(:,find(strcmp(A1,'forceplate2_py')>0)-1);
vz1 = y1(:,find(strcmp(A1,'forceplate1_vz')>0)-1);
vz2 = y1(:,find(strcmp(A1,'forceplate2_vz')>0)-1);

on1 = vz1 > 60;  % Can adjust these if needed
on2 = vz2 > 60;

close all;

figure(5)
plot(lheelx(on1)-px1(on1));
hold on; grid on;
plot(ltoex(on1)-px1(on1),'r');

figure(6)
plot(lmedy(on1)-py1(on1),'k');
hold on; grid on;
plot(llaty(on1)-py1(on1),'m');

figure(7)
plot(rheelx(on2)-px2(on2));
hold on; grid on;
plot(rtoex(on2)-px2(on2),'r');

figure(8)
plot(rmedy(on2)-py2(on2),'k');
hold on; grid on;
plot(rlaty(on2)-py2(on2),'m');

keyboard

%figure(1)
%plot(lheelz);
%hold on; grid on;
%plot(ltoez,'r');
%plot(llatz,'m');
%plot(lmedz,'k');

%figure(2);
%plot(rheelz);
%hold on; grid on;
%plot(rtoez,'r');
%plot(rlatz,'m');
%plot(rmedz,'k');

for i = 1:length(lheelx)
  
  figure(3)
  plot(lheelx(i),lheely(i),'rx');
  hold on; grid on;
  plot(ltoex(i),ltoey(i),'r+');
  plot(lmedx(i),lmedy(i),'*');
  plot(llatx(i),llaty(i),'o');
  plot(px1(i),py1(i),'k.'); 

  figure(4)
  plot(rheelx(i),rheely(i),'rx');
  hold on; grid on;
  plot(rtoex(i),rtoey(i),'r+');
  plot(rmedx(i),rmedy(i),'*');
  plot(rlatx(i),rlaty(i),'o');
  plot(px2(i),py2(i),'k.'); 

  if mod(i,100) == 1
    keyboard
  end;
  
end;
