function [F] = idynfun(x);

% perform one inverse dynamics analysis, and return RMS difference between
% resultant joint loads and known results

% By: Ton van den Bogert, University of Calgary
% Date: April 26, 1996
% Report problems to bogert@acs.ucalgary.ca

global eval dt kin frc mom fmg n m I fCM FMhip FMknee FMankle Mhip Mknee Mankle

eval = eval + 1;
ordkin = 2;
ordfrc = 2;

% do the filtering according to parameters x
% 1 = thigh cut-off
% 2 = shank cut-off
% 3 - foot cut-off
% 4 = force cut-off

fkin = kin;
if (x(4) == 0)
  ffrc = frc;
else
  ffrc = matfiltfilt(dt, x(4), ordfrc, frc);
end

% extract variables from the matrices
Rhip = fkin(:,1:2);
Rknee = fkin(:,3:4);
Rankle = fkin(:,5:6);
Rtoe = fkin(:,7:8);
Fgrf = ffrc(:,1:2);
Rgrf = [ffrc(:,3), zeros(n,1)];
Mgrf = zeros(n,1);

% calculate segment angles
A(:,1) = atan2(Rhip(:,2)  - Rknee(:,2),  Rhip(:,1)  - Rknee(:,1));	% thigh
A(:,2) = atan2(Rknee(:,2) - Rankle(:,2), Rknee(:,1) - Rankle(:,1));	% shank
A(:,3) = atan2(Rankle(:,2)- Rtoe(:,2),   Rankle(:,1)- Rtoe(:,1));	% foot

% remove -pi to pi transitions
for i=2:n
  for j=1:3
    while (A(i,j)-A(i-1,j) > 2.5),
	A(i,j) = A(i,j) - 2*pi;
    end
    while (A(i,j)-A(i-1,j) < -2.5),
	A(i,j) = A(i,j) + 2*pi;
    end
  end
end

% calculate segment CM positions
R(:,1:2) = Rhip + fCM(1)*(Rknee - Rhip);		% thigh CM
R(:,3:4) = Rknee + fCM(2)*(Rankle - Rknee);		% shank CM
R(:,5:6) = Rankle + fCM(3)*(Rtoe - Rankle);		% foot CM

% filter kinematic data
if (x(1) ~= 0)
  R(:,1:2) = matfiltfilt(dt, x(1), ordkin, R(:,1:2));
  A(:,1) = matfiltfilt(dt, x(1), ordkin, A(:,1));
end
if (x(2) ~= 0)
  R(:,3:4) = matfiltfilt(dt, x(2), ordkin, R(:,3:4));
  A(:,2) = matfiltfilt(dt, x(2), ordkin, A(:,2));
end
if (x(3) ~= 0)
  R(:,5:6) = matfiltfilt(dt, x(3), ordkin, R(:,5:6));
  A(:,3) = matfiltfilt(dt, x(3), ordkin, A(:,3));
end

% segment angular accelerations
Add(1,:) = [0,0,0];
for i=2:n-1
  Add(i,:) = (A(i+1,:) - 2*A(i,:) + A(i-1,:))/dt^2;
end
Add(n,:) = [0,0,0];

% calculate accelerations using 3-point finite differences
Rdd(1,:) = [0,0,0,0,0,0];
for i=2:n-1
  Rdd(i,:) = (R(i+1,:) - 2*R(i,:) + R(i-1,:))/dt^2;
end
Rdd(n,:) = [0,0,0,0,0,0];

% uncomment these for a quasi-static analysis
%Rdd = zeros(size(Rdd));
%Add = zeros(size(Add));

% do the inverse dynamics for foot, shank, thigh
[Fankle, Mankle] = inv2d(R(:,5:6), Rankle, Rgrf,   Fgrf  , Mgrf,   Rdd(:,5:6), Add(:,3), m(3), I(3), fCM(3));
[Fknee , Mknee ] = inv2d(R(:,3:4), Rknee , Rankle, Fankle, Mankle, Rdd(:,3:4), Add(:,2), m(2), I(2), fCM(2));
[Fhip  , Mhip  ] = inv2d(R(:,1:2), Rhip  , Rknee,  Fknee , Mknee,  Rdd(:,1:2), Add(:,1), m(1), I(1), fCM(1));

% optional filter of inverse dynamics results
order = 2;
fcut = 0;
if (fcut>0)
  fcut = fcut/(sqrt(2)-1)^(0.5/order);
  [b,a] = butter(order,2*fcut*dt);
  Fankle(:,1) = filtfilt(b,a,Fankle(:,1));
  Fknee(:,1) = filtfilt(b,a,Fknee(:,1));
  Fhip(:,1) = filtfilt(b,a,Fhip(:,1));
  Fankle(:,2) = filtfilt(b,a,Fankle(:,2));
  Fknee(:,2) = filtfilt(b,a,Fknee(:,2));
  Fhip(:,2) = filtfilt(b,a,Fhip(:,2));
  Mankle = filtfilt(b,a,Mankle);
  Mknee = filtfilt(b,a,Mknee);
  Mhip = filtfilt(b,a,Mhip);
end;

% calculate magnitude of intersegmental forces
for i=1:n;
  FMhip(i) = norm(Fhip(i,:));
  FMknee(i) = norm(Fknee(i,:));
  FMankle(i) = norm(Fankle(i,:));
end

Mknee = -Mknee;

% only evaluate RMS on middle part of the 100 data points
t1 = 30;
t2 = 90;
% calculate RMS errors for moments:
rmsmom(1) = norm(Mhip(t1:t2)-mom(t1:t2,1))/sqrt(t2-t1+1);
rmsmom(2) = norm(Mknee(t1:t2)-mom(t1:t2,2))/sqrt(t2-t1+1);
rmsmom(3) = norm(Mankle(t1:t2)-mom(t1:t2,3))/sqrt(t2-t1+1);
%plot(t1:t2,Mhip(t1:t2),t1:t2,mom(t1:t2,1)); drawnow;

% calculate RMS errors for forces:
rmsfrc(1) = norm(FMhip(t1:t2)'-fmg(t1:t2,1))/sqrt(t2-t1+1);
rmsfrc(2) = norm(FMknee(t1:t2)'-fmg(t1:t2,2))/sqrt(t2-t1+1);
rmsfrc(3) = norm(FMankle(t1:t2)'-fmg(t1:t2,3))/sqrt(t2-t1+1);
%plot(t1:t2,FMhip(t1:t2),t1:t2,fmg(t1:t2,1)); drawnow;

% Change the following line for your own optimization criterion.  E.g. to optimize
% for best resultant ankle force magnitude: F = rmsfrc(3);
% for best forces in all joints combined: F = norm(rmsfrc)/sqrt(3)

F = norm(rmsmom)/sqrt(3);	% optimize for best moments (all joints combined)

fprintf(1,'%d: Fcut:%6.3f %6.3f %6.3f %6.3f F:%6.4f RMSmom: %5.3f %5.3f %5.3f RMSfrc: %5.3f %5.3f %5.3f\n', ...
  eval,x,F,rmsmom,rmsfrc);