function edit_fp1(inputfile,path1,rezero,lowpass,truncate)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 
% edit_fp1.m prepares the force plate treadmill data gathered from
% the Harvard/Bertec setup for processing in SIMM.  The force plate
% data is taken from an input .mat file, corrected, and added to a
% SIMM .mot file.  Note that the treadmill height and force plate
% positions are specific to this setup.  
%
% Inputs:
% inputfile - input file name (without .mat or .mot)
% path1     - subject initials and date
% rezero    - 0 if force plate rezeroing is desired, 1 otherwise
% lowpass   - 0 if no smoothing desired, 2*Fc/Fs otherwise 
% truncate  - -1 if no truncation, [start stop] in s otherwise.
%
% Cropping of SIMM files is accounted for; output time stamps are
% the same as given by SIMM.  Note that if truncation is desired 
% the final range must be within the cropped range of the SIMM .mot
% files.
%
% Outputs:
% Produces new .mot file with force plate data ready for processing
% in SIMM.
%
% Example Usage:
% edit_fp1('walk175_3','EM072611',1,.1,[0 19])
%
% Jared Markowitz
% Summer 2011
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Load original .mot file

fprintf(['Input file: ' inputfile '\n']);

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

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

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

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

status = fclose('all');

ncol = size(y1,2)+1;

A1 = cellstr(labels1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Load .mat file

cmd1 = ['load ''' path1 'mat/' inputfile '.mat'';'];
eval(cmd1);
cmd2 = ['FP1 = ' inputfile '.Force(1);'];
eval(cmd2);
cmd3 = ['FP2 = ' inputfile '.Force(2);'];
eval(cmd3);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Re-zero
h = .0143;

close all;
figure(3)
plot(FP1.Force(3,:));
axis([0 length(FP1.Force(3,:)) -40 70]);
grid on;
figure(4)
plot(FP2.Force(3,:),'r');
axis([0 length(FP2.Force(3,:)) -40 70]);
grid on
thresh1 = input('Please input vector of force plate thresholds. Blue is FP1, Red is FP2.\n:');

off1 = find(FP1.Force(3,:) < thresh1(1));
off2 = find(FP2.Force(3,:) < thresh1(2));

if rezero == 1
  vx1_fp  = FP1.Force(1,:)-mean(FP1.Force(1,off1));
  vy1_fp  = FP1.Force(2,:)-mean(FP1.Force(2,off1));
  vz1_fp  = FP1.Force(3,:)-mean(FP1.Force(3,off1));
  mx1_fpc = FP1.Moment(1,:)-mean(FP1.Moment(1,off1));
  my1_fpc = FP1.Moment(2,:)-mean(FP1.Moment(2,off1));
  mz1_fpc = FP1.Moment(3,:)-mean(FP1.Moment(3,off1));

  vx2_fp  = FP2.Force(1,:)-mean(FP2.Force(1,off2));
  vy2_fp  = FP2.Force(2,:)-mean(FP2.Force(2,off2));
  vz2_fp  = FP2.Force(3,:)-mean(FP2.Force(3,off2));
  mx2_fpc = FP2.Moment(1,:)-mean(FP2.Moment(1,off2));
  my2_fpc = FP2.Moment(2,:)-mean(FP2.Moment(2,off2));
  mz2_fpc = FP2.Moment(3,:)-mean(FP2.Moment(3,off2));
else
  vx1_fp  = FP1.Force(1,:);
  vy1_fp  = FP1.Force(2,:);
  vz1_fp  = FP1.Force(3,:);
  mx1_fpc = FP1.Moment(1,:);
  my1_fpc = FP1.Moment(2,:);
  mz1_fpc = FP1.Moment(3,:);

  vx2_fp  = FP2.Force(1,:);
  vy2_fp  = FP2.Force(2,:);
  vz2_fp  = FP2.Force(3,:);
  mx2_fpc = FP2.Moment(1,:);
  my2_fpc = FP2.Moment(2,:);
  mz2_fpc = FP2.Moment(3,:);
end;

%compare1

% Center of FP frame
px1_fp = 1000*(-h*vx1_fp-my1_fpc)./vz1_fp+250;
py1_fp = 1000*(-h*vy1_fp+mx1_fpc)./vz1_fp-750;
pz1_fp = 1000*.0143*ones(size(px1_fp));
px2_fp = 1000*(-h*vx2_fp-my2_fpc)./vz2_fp-250;
py2_fp = 1000*(-h*vy2_fp+mx2_fpc)./vz2_fp-750;
pz2_fp = 1000*.0143*ones(size(px2_fp));

mz1_fp = mz1_fpc + 0.25*vy1_fp + 0.75*vx1_fp;
mz2_fp = mz2_fpc - 0.25*vy2_fp + 0.75*vx2_fp;

% The following are also the values in the lab frame, b/c
% the fact that they are forces exerted on FP cancels the 
% minus sign in coordinate change.  See compare1.m for details.

tz1_lab = mz1_fp - px1_fp.*vy1_fp/1000 + py1_fp.*vx1_fp/1000;
tz2_lab = mz2_fp - px2_fp.*vy2_fp/1000 + py2_fp.*vx2_fp/1000;

% Convert to lab frame:
px1_lab = (py1_fp-83.258)/1000;
py1_lab = (px1_fp-108.636)/1000;
vx1_lab = -vy1_fp;
vy1_lab = -vx1_fp;
vz1_lab = vz1_fp;

px2_lab = (py2_fp-83.258)/1000;
py2_lab = (px2_fp-592.774)/1000;
vx2_lab = -vy2_fp;
vy2_lab = -vx2_fp;
vz2_lab = vz2_fp;

px1_lab(off1) = 0;
py1_lab(off1) = 0;
px2_lab(off2) = 0;
py2_lab(off2) = 0;

tz1_lab(off1) = 0;
tz2_lab(off2) = 0;

% Low pass filter:

if lowpass > 0
  [b,a] = butter(2,lowpass);  %.10 (1000 Hz is Fs)
  
  px1_lab = filtfilt(b,a,px1_lab);
  py1_lab = filtfilt(b,a,py1_lab);
  vx1_lab = filtfilt(b,a,vx1_lab);
  vy1_lab = filtfilt(b,a,vy1_lab);
  vz1_lab = filtfilt(b,a,vz1_lab);
  px2_lab = filtfilt(b,a,px2_lab);
  py2_lab = filtfilt(b,a,py2_lab);
  vx2_lab = filtfilt(b,a,vx2_lab);
  vy2_lab = filtfilt(b,a,vy2_lab);
  vz2_lab = filtfilt(b,a,vz2_lab);

  tz1_lab = filtfilt(b,a,tz1_lab);
  tz2_lab = filtfilt(b,a,tz2_lab);
  
end;

% Both SIMM and QTM timing start at 0s, go to (end - 1/Fs).
% Account for possible cropping in SIMM:

if length(px1_lab) > length(x1)*8
  Fs1 = 1000;
  x2  = 0:1/Fs1:(length(px1_lab)-1)/Fs1;  
  [val1,ind1] = min(abs(x1(1)-x2));
  [val2,ind2] = min(abs(x1(end)-x2));
  off11 = find((off1>=ind1) & (off1<=ind2));  
  off22 = find((off2>=ind1) & (off2<=ind2)); 
  off1  = off1(off11)-(ind1-1)*ones(size(off1(off11)));
  off2  = off2(off22)-(ind1-1)*ones(size(off2(off22)));
else
  ind1 = 1;
  ind2 = length(px1_lab);
end;    

% Downsample:
px1_lab = resample(px1_lab(ind1:ind2),1,8);
py1_lab = resample(py1_lab(ind1:ind2),1,8);
vx1_lab = resample(vx1_lab(ind1:ind2),1,8);
vy1_lab = resample(vy1_lab(ind1:ind2),1,8);
vz1_lab = resample(vz1_lab(ind1:ind2),1,8);
px2_lab = resample(px2_lab(ind1:ind2),1,8);
py2_lab = resample(py2_lab(ind1:ind2),1,8);
vx2_lab = resample(vx2_lab(ind1:ind2),1,8);
vy2_lab = resample(vy2_lab(ind1:ind2),1,8);
vz2_lab = resample(vz2_lab(ind1:ind2),1,8);

tz1_lab = resample(tz1_lab(ind1:ind2),1,8);
tz2_lab = resample(tz2_lab(ind1:ind2),1,8);

tx1_lab = zeros(size(tz1_lab));
ty1_lab = zeros(size(tz1_lab));
tx2_lab = zeros(size(tz2_lab));
ty2_lab = zeros(size(tz2_lab));

off1 = ceil(off1/8);
off2 = ceil(off2/8);

pz1_lab = (.0143-.033731)*ones(size(px1_lab));
pz2_lab = (.0143-.033731)*ones(size(px2_lab));

px1_lab(off1) = 0;
py1_lab(off1) = 0;
pz1_lab(off1) = 0;
vx1_lab(off1) = 0;
vy1_lab(off1) = 0;
vz1_lab(off1) = 0;
px2_lab(off2) = 0;
py2_lab(off2) = 0;
pz2_lab(off2) = 0;
vx2_lab(off2) = 0;
vy2_lab(off2) = 0;
vz2_lab(off2) = 0;

tz1_lab(off1) = 0;
tz2_lab(off2) = 0;

% Create new output array:

y2=[x1 y1 px1_lab' py1_lab' pz1_lab' vx1_lab' vy1_lab' vz1_lab' tx1_lab' ty1_lab' tz1_lab' px2_lab' py2_lab' pz2_lab' vx2_lab' vy2_lab' vz2_lab' tx2_lab' ty2_lab' tz2_lab'];

% Account for possible external truncation (due to lost markers, etc):

if sum(truncate==-1) == 0
  Fs = 125;
  if truncate(1) ~= 0
    [vala,inda] = min(abs(x1-truncate(1)));
  else
    inda = 1;
  end;
  if truncate(2)>60
    indb = size(y2,1);
  else
    [valb,indb] = min(abs(x1-truncate(2)));
  end;
  y2 = y2(inda:indb,:);    
  % Don't implement the following so that
  % time stamps can be used to track EMG!
  % y2(:,1) = 0:(1/Fs):((indb-inda)/Fs);
  % y2(:,2) = 2:4:(4*(indb-inda)+2);
end;  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Concatenate and write columns to output file:

fid1 = fopen([path1 'original_mot/' inputfile '.mot'],'r');
fid2 = fopen([path1 'edited_mot/' inputfile '.mot'],'w');

label_line = 0;
fp_line    = 1;

while label_line == 0
  tline = fgetl(fid1);
  if length(tline) >= 11 && strcmp(tline(1:11),'datacolumns') == 1
    ncol = str2num(tline(13:end));
    tline = cat(2,tline(1:12),char(num2str(ncol+18)));
  end;
  if length(tline) >= 8 && strcmp(tline(1:8),'datarows') == 1
    tline = cat(2,tline(1:9),char(num2str(size(y2,1))));
  end;  
  if strcmp(tline,'endheader') == 1
    tline1 = ['forceplate 0.500000 1.500000 0.000000 -.083258 -0.592774 -0.019431 -180.0000 0.000000 -90.00000'];
    tline2 = ['forceplate 0.500001 1.500000 0.000000 -.083258 -0.108636 -0.019431 -180.0000 0.000000 -90.00000'];         
    fprintf(fid2,'%s \n',tline1);    
    fprintf(fid2,'%s \n',tline2);
  end;  
  if length(tline) >= 4
    label_line = strcmp(tline(1:4),'time');
  end;
  
  if label_line == 1
    tline = sprintf([tline 'forceplate1_px\tforceplate1_py\tforceplate1_pz\tforceplate1_vx\tforceplate1_vy\tforceplate1_vz\tforceplate1_torque_x\tforceplate1_torque_y\tforceplate1_torque_z\tforceplate2_px\tforceplate2_py\tforceplate2_pz\tforceplate2_vx\tforceplate2_vy\tforceplate2_vz\tforceplate2_torque_x\tforceplate2_torque_y\tforceplate2_torque_z']);  
  end;

  fprintf(fid2,'%s \n',tline);

end;  

format_str = [];
% Forgot the + 18 previously but rigorously showed that it didn't
% affect output data.  Just had new lines in different places but
% both SIMM and MATLAB read it correctly.  Phew!
for j=1:(ncol+18)
  format_str = strcat(format_str,'   %8.6f');
end;

format_str = strcat(format_str,'\n');

fprintf(fid2,format_str,y2');

status = fclose('all');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
