% This is a sanity check script to validate GRF torque calculations.
% Verified that tz1_fp1, tz2_fp2 here are identical to tz1_lab, tz2_lab in edit_fp2.m

% Compute Tz in corner frame:
px1_fp = (-h*vx1_fp-my1_fpc)./vz1_fp;
py1_fp = (-h*vy1_fp+mx1_fpc)./vz1_fp;
pz1_fp = .0147*ones(size(px1_fp));
px2_fp = (-h*vx2_fp-my2_fpc)./vz2_fp;
py2_fp = (-h*vy2_fp+mx2_fpc)./vz2_fp;
pz2_fp = .0147*ones(size(px2_fp));

tz1_fp1 = mz1_fpc - px1_fp.*vy1_fp + py1_fp.*vx1_fp;
tz2_fp1 = mz2_fpc - px2_fp.*vy2_fp + py2_fp.*vx2_fp;

figure(12)
plot(tz1_fp1);
hold on; grid on;
figure(13)
plot(tz2_fp1);
hold on; grid on;

% Compute Tz in center 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*.0147*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*.0147*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;
tz1_fp2 = mz1_fp - px1_fp.*vy1_fp/1000 + py1_fp.*vx1_fp/1000;
tz2_fp2 = mz2_fp - px2_fp.*vy2_fp/1000 + py2_fp.*vx2_fp/1000;

figure(12)
plot(tz1_fp2,'r');
hold on; grid on;
figure(13)
plot(tz2_fp2,'r');
hold on; grid on;

% Compute Tz in lab frame:
% First find center of force plate in lab frame:
center1 = mean(FP1.ForcePlateLocation,1);
center2 = mean(FP2.ForcePlateLocation,1);

px1_lab = (-px1_fp+center1(1))/1000;
py1_lab = (py1_fp+center1(2))/1000;
vx1_lab = -vx1_fp;     % still force exerted on FP, just diff coords
vy1_lab = vy1_fp;
vz1_lab = -vz1_fp;

px2_lab = (-px2_fp+center2(1))/1000;
py2_lab = (py2_fp+center2(2))/1000;
vx2_lab = -vx2_fp;
vy2_lab = vy2_fp;
vz2_lab = -vz2_fp;

mz1_lab = -mz1_fp + center1(1)/1000*vy1_lab - center1(2)/1000*vx1_lab;
mz2_lab = -mz2_fp + center2(1)/1000*vy2_lab - center2(2)/1000*vx2_lab;

tz1_lab = mz1_lab - px1_lab.*vy1_lab + py1_lab.*vx1_lab;
tz2_lab = mz2_lab - px2_lab.*vy2_lab + py2_lab.*vx2_lab;

figure(12)
plot(-tz1_lab,'k');
hold on; grid on;
figure(13)
plot(-tz2_lab,'k');
hold on; grid on;

figure(14)
plot(-tz1_lab-tz1_fp1);
hold on;
plot(-tz2_lab-tz2_fp2,'r');
grid on;

[b,a] = butter(2,lowpass);

tz1_lab(off1)= 0;
tz2_lab(off2)= 0;
tz1_lab = filtfilt(b,a,tz1_lab);
tz2_lab = filtfilt(b,a,tz2_lab);
tz1_lab(off1)= 0;
tz2_lab(off2)= 0;

figure(15)
plot(-tz1_lab);
hold on;
plot(-tz2_lab,'r');

clear p*_fp; clear mz*_fp; clear tz*lab;