function [ Fm ] = muscle_force( M, r, p, A, Fmax)

% purpose - to estimate individual muscle forces using static optimization, but solved
using a Lagrangian multiplier technique.
% -------
%
% John H. Challis (May 20, 1999)
%
% calling - [ Fm ] = muscle_force( M, r, p, A, Fmax)
% -------
%
% input
% -----
% M - moment at joint to be satisfied.
% r - array of moment arms of muscles
% p - power of objective function
% A - array of divisors for objective function [optional]
% Fmax - array of maximum force muscles can produce [optional]
%
% output
% ------
% Fm - array of muscle forces, which satisfy the constraints and minimize the objective
function.
%
% notes
% -----
% 1) The objective function to be minimized is: ( Fm(1) / A(1) )^p + ( Fm(2) / A(2) )^p
+..... ( Fm(n) / A(n) )^p
% 2) The constraint to be satisfied is: M = ( Fm(1) * r(1) ) + ( Fm(2) * r(2) ) +.....
( Fm(n) * r(n) )
% 3) Equality constraint: Fm(i) >= 0
% Equality constraint: Fm(i) <= Fmax(i) [optional]
% 4) The array A could contain the muscle cross-sectional areas (minimize muscle stress)
% the maximum muscle forces (minimize relative muscle
force)
% 5) Solutions are only approximate if the objective function is linear (p = 1)
% 6) All input variables should be in SI units.
% 7) The algorithm for this calculation is described in,
% Challis, J. H. & D. G. Kerwin. 1993. An analytical examination of muscle force
estimations using optimization techniques.
% Journal of Engineering in Medicine. 207: 139-148.
%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% set some initial parameters %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if p == 1
p = 1.001;
end
%
p1 = p / (p - 1);
%
n = length( r );

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% check number of inputs, set those not used %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin == 3
A(1:n) = ones(n,1);
end
%
if nargin == 3 | nargin ==4
for i=1:n
Fmax(i) = ( M / r(i) );
end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% check feasible solution exists %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Mp = 0;
for i=1:n
Mp = r(i) * Fmax(i) + Mp;
end
%
if Mp < M
disp('No feasible solution exists....')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% now estimate muscle forces %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:n
ra(i) = r(i) * A(i);
end
%
for j=1:n
bot = 0;
for i=1:n
bot = ( ( ( ra(i) / ra(j) )^p1 ) ) + bot;
end
%
Fm(j) = M / ( r(j) * bot );
%
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% adjust if maximum forces have been exceeded %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F_OK = find( (Fm - Fmax) <= 0);
F_not_OK = find( (Fm - Fmax) > 0);
F_Fixed = [];

while F_not_OK ~ []
%
Mnew = M;
% adjust Mnew for values set to maximum
%
n = length( F_Fixed );
if n > 0
for i=1:n
j = F_Fixed(i);
Mnew = Mnew - ( Fm(j) * r(j) );
end
end
% set Fm greater than Fmax to their maximums
%
n = length( F_not_OK );
for i=1:n
j = F_not_OK(i);
Fm(j) = Fmax(j);
Mnew = Mnew - ( Fm(j) * r(j) );
end
% re-estimate remaining values
%
n = length( F_OK );
%
for j=1:n
k = F_OK(j);
bot = 0;
for i=1:n
l = F_OK(i);
bot = ( ( ( ra(l) / ra(k) )^p1 ) ) + bot;
end
%
Fm(k) = Mnew / ( r(k) * bot );
%
end
% check that with new estimate of remaining values, the maximums have not been exceeded
%
F_Fixed = find( Fm == Fmax );
F_OK = find( (Fm - Fmax) < 0);
F_not_OK = find( (Fm - Fmax) > 0);
%
end

%
%%
%%% The End %%%
%%
%