function [Fjx,Fjy, Fm] = BioMP1(aa, ak, aw, as, H, M, mobj)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Function file: BioMP1.m
%Purpose:
%   To determine location of the normal force for a person in a certain
%   pose with a certain mass and the joint forces on the hip along with the
%   force of the muscle erector spinae
%   All angle inputs are interior
%Record of revisions (Date | Programmer | Change):
%   2/15/20| Andrew Bodney | Original program
%Main Variables:
%INPUTS:
%   aa         -angle at ankle in degrees
%   ak         -angle at knee in degrees
%   ah         -angle at hips in degrees
%   as         -angle at shoulder in degrees
%   H          -height of person in meters
%   M          -mass of person in kilograms
%   mobj         -mass of object in kilograms
%OUTPUTS:
%   Fjx        -Joint force on hip in x-direction
%   Fjy        -Joint force on hip in y-direction
%   Fm         -Force of muscle erector spinae
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%initial
%origin is toes
g = 9.81; %gravity
%object position in meters
x = 0.25;
y = 0.5;
%angles from degrees to radians
aground = 180*pi/180;
aankle = aa*pi/180;
aknee = ak*pi/180;
awaist = aw*pi/180;
ashoulder = as*pi/180;
%lengths of body parts
lfeet = (0.152)*H;
hfeet = (0.039)*H;
llegs = (0.285-0.039)*H;
lthighs = 0.245*H;
ltnh = (0.818-0.53)*H;
lua = 0.186*H;
lfah = (0.145+0.108/2)*H;
%masses of body parts
mfeet = 0.0145*2*M;
mlegs = 0.0465*2*M;
mthighs = 0.1*2*M;
mtnh = 0.578*M;
mua = 0.028*2*M;
mfah = 0.022*2*M;
msum = mfeet+mlegs+mthighs+mtnh+mua+mfah+mobj;
%Weight matrix
Wmfeet = [0; -mfeet*g; 0];
Wmlegs = [0; -mlegs*g; 0];
Wmthighs = [0; -mthighs*g; 0];
Wmtnh = [0; -mtnh*g; 0];
Wmua = [0; -mua*g; 0];
Wmfah = [0; -mfah*g; 0];
Wmsum = [0; -msum*g; 0];
%Weight as a force in y-direction
Wfeet = mfeet*g;
Wlegs = mlegs*g;
Wthighs = mthighs*g;
Wtnh = mtnh*g;
Wua = mua*g;
Wfah = mfah*g;
Wsum = msum*g;
%center of gravity of body parts
cgfeet = 0.095*H;
cglegs = 0.139*H;
cgthighs = 0.139*H;
%swap from distal to proximal measuring after hip
cgtnh = 0.190*H;
cgua = 0.081*H;
cgfah = 0.099*H;
cgobj = x;
%Normal force
FN = [0; Wsum; 0];
rFN = [-0.5*0.152*H 0 0];
%Location of Normal force on the feet
lN = (Wfeet*cgfeet + Wlegs*cglegs + Wthighs*cgthighs + Wtnh*cgtnh + Wua*cgua + Wfah*cgfah + mobj*g*cgobj)./FN;
%Set up for the rotation matrices
Rground = [cos(aground) -sin(aground) 0 0; sin(aground) cos(aground) 0 0; 0 0 1 0; 0 0 0 1];
Rankle = [cos(aankle) -sin(aankle) 0 0; sin(aankle) cos(aankle) 0 0; 0 0 1 0; 0 0 0 1];
Rknee = [cos(aknee) -sin(aknee) 0 0; sin(aknee) cos(aknee) 0 0; 0 0 1 0; 0 0 0 1];
Rwaist = [cos(awaist) -sin(awaist) 0 0; sin(awaist) cos(awaist) 0 0; 0 0 1 0; 0 0 0 1];
Rshoulder = [cos(ashoulder) -sin(ashoulder) 0 0; sin(ashoulder) cos(ashoulder) 0 0; 0 0 1 0; 0 0 0 1];
%Set up for the lengths as matrices
Lfeet = [1 0 0 lfeet; 0 1 0 -hfeet; 0 0 1 0; 0 0 0 1];
Llegs = [1 0 0 llegs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lthighs = [1 0 0 lthighs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Ltnh = [1 0 0 ltnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lua = [1 0 0 lua; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lfah = [1 0 0 lfah; 0 1 0 0; 0 0 1 0; 0 0 0 1];
%Create secondary link matrices for the distance to the CG on each link
Lfeetcg = [1 0 0 cgfeet; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Llegscg = [1 0 0 cglegs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lthighscg = [1 0 0 cgthighs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Ltnhcg = [1 0 0 cgtnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Luacg = [1 0 0 cgua; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lfahcg = [1 0 0 cgfah; 0 1 0 0; 0 0 1 0; 0 0 0 1];
%Displacement of x-value for each segment
Tfeet = (Rground * Lfeet);
Tlegs = (Rground*Lfeet*Rankle*Llegs);
Tthighs = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs);
Ttnh = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh);
Tua = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua);
Tfah = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
Tobj = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
%Calculate the x-value of displacement for each CG
Tfeetcg = (Rground * Lfeetcg);
Tlegscg = (Rground*Lfeet*Rankle*Llegscg);
Tthighscg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighscg);
Ttnhcg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnhcg);
Tuacg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Luacg);
Tfahcg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfahcg);
Tobj = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
%Define vectors from the origin (where we will take the moments) to the CGs and the object.
rfeetcg = [Tfeetcg(1, 4), Tfeetcg(2, 4), 0];
rlegscg = [Tlegscg(1, 4), Tlegscg(2, 4), 0];
rthighscg = [Tthighscg(1, 4), Tthighscg(2, 4), 0];
rtnhcg = [Ttnhcg(1, 4), Ttnhcg(2, 4), 0];
ruacg = [Tuacg(1, 4), Tuacg(2, 4), 0];
rfahcg = [Tfahcg(1, 4), Tfahcg(2, 4), 0];
robj = [Tobj(1, 4), Tobj(2, 4), 0];
%Define unit vectors
Lesp = [1 0 0 0; 0 1 0 0.05*ltnh; 0 0 1 0; 0 0 0 1];
Lesd = [1 0 0 0.8*ltnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Tesp = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Lesp);
Tesd = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Lesd);
%now move to the spine
%Swapped definition of proximal and distal as we move away from the hips
Fmdir = [Tesd(1,4)-Tesp(1,4) Tesd(2,4)- Tesp(2,4) 0];
Fmdir = Fmdir/norm(Fmdir);
rFm = [Tesp(1,4) Tesp(2,4) 0];
rthighs = [Tthighscg(1,4) Tthighscg(2,4) 0];
msum = (cross(rfeetcg,Wmfeet)+cross(rlegscg,Wmlegs)+cross(rthighscg,Wmthighs)+cross(rFN,FN)); msum = msum(3);
RHS = [0; mfeet*g+mlegs*g+mthighs*g-(M+mobj)*g; -msum];
Fmmom = cross(rFm, Fmdir);
LHS = [1 0 Fmdir(1); 0 1 Fmdir(2); -rthighs(2) rthighs(1) Fmmom(3)];
soln = inv(LHS)*RHS;
Fjx = soln([1]);
Fjy = soln([2]);
Fm = soln([3]);