function dz = dynamics_3_link(~,z,P) 
%DZ = DYNAMICS_3_LINK(T,Z,P)
% 
%FUNCTION:  This function computes the dynamics of a 3
%    link pendulum, and is designed to be called from ode45.
%    The model allows for arbitrary mass and inertia for each
%    link, but no friction or actuation
% 
%INPUTS: 
%    t = time. Dummy input for ode45. Not used.
%    z = [6 X nTime]  matrix of states
%    P = struct of parameters
%OUTPUTS: 
%    dz = [6 X nTime]  matrix of state derivatives
% 
%NOTES:
%    This file was automatically generated by writeDynamics.m

g  = P.g ; %gravity
m1 = P.m(1); % Link 1 mass
m2 = P.m(2); % Link 2 mass
m3 = P.m(3); % Link 3 mass
l1 = P.l(1); % Link 1 length
l2 = P.l(2); % Link 2 length
l3 = P.l(3); % Link 3 length
I1 = P.I(1); % Link 1 moment of inertia about its center of mass
I2 = P.I(2); % Link 2 moment of inertia about its center of mass
I3 = P.I(3); % Link 3 moment of inertia about its center of mass
d1 = P.d(1); % Link 1 distance between center of mass and parent joint
d2 = P.d(2); % Link 2 distance between center of mass and parent joint
d3 = P.d(3); % Link 3 distance between center of mass and parent joint

nTime = size(z,2);
dz = zeros(size(z)); 
M = zeros(3,3,nTime);
f = zeros(3,nTime);

th1 = z(1,:); 
th2 = z(2,:); 
th3 = z(3,:); 

dth1 = z(4,:); 
dth2 = z(5,:); 
dth3 = z(6,:); 

dz(1,:) = dth1; 
dz(2,:) = dth2; 
dz(3,:) = dth3; 

M(1,1,:) = - I1 - d1.^2.*m1 - l1.^2.*m2 - l1.^2.*m3;
M(1,2,:) = - d2.*l1.*m2.*cos(th1 - th2) - l1.*l2.*m3.*cos(th1 - th2);
M(1,3,:) = -d3.*l1.*m3.*cos(th1 - th3);
M(2,1,:) = - d2.*l1.*m2.*cos(th1 - th2) - l1.*l2.*m3.*cos(th1 - th2);
M(2,2,:) = - I2 - d2.^2.*m2 - l2.^2.*m3;
M(2,3,:) = -d3.*l2.*m3.*cos(th2 - th3);
M(3,1,:) = -d3.*l1.*m3.*cos(th1 - th3);
M(3,2,:) = -d3.*l2.*m3.*cos(th2 - th3);
M(3,3,:) = - I3 - d3.^2.*m3;

f(1,:) = - d1.*g.*m1.*cos(th1) - g.*l1.*m2.*cos(th1) - g.*l1.*m3.*cos(th1) - d2.*dth2.^2.*l1.*m2.*sin(th1 - th2) - d3.*dth3.^2.*l1.*m3.*sin(th1 - th3) - dth2.^2.*l1.*l2.*m3.*sin(th1 - th2);
f(2,:) = d2.*dth1.^2.*l1.*m2.*sin(th1 - th2) - g.*l2.*m3.*cos(th2) - d2.*g.*m2.*cos(th2) - d3.*dth3.^2.*l2.*m3.*sin(th2 - th3) + dth1.^2.*l1.*l2.*m3.*sin(th1 - th2);
f(3,:) = d3.*dth1.^2.*l1.*m3.*sin(th1 - th3) - d3.*g.*m3.*cos(th3) + d3.*dth2.^2.*l2.*m3.*sin(th2 - th3);

for i=1:nTime 
    MM = M(:,:,i);  ff = f(:,i); 
    dz(4:6,i) = -MM \ ff;
end 

end 
