function dz = doublePendulumDynamics(~,z,u,P) 
%DZ = DOUBLEPENDULUMDYNAMICS(T,Z,U,P)
% 
%FUNCTION:  This function computes the dynamics of a double
%    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 = [4xn] matrix of states.
%    u = [2xn] matrix of inputs
%    P = struct of parameters
%OUTPUTS: 
%    dz = [4xn] matrix of state derivatives
% 
%NOTES:
%    This file was automatically generated by writeDoublePendulumDynamics

m1 = P.m1; %link one mass
m2 = P.m2; %link two mass
g  = P.g ; %gravity
l1 = P.l1; %link one length
l2 = P.l2; %link two length
I1 = P.I1; %link one moment of inertia about its center of mass
I2 = P.I2; %link two moment of inertia about its center of mass
d1 = P.d1; %distance between link one center of mass and parent joint
d2 = P.d2; %distance between link two center of mass and parent joint

th1 = z(1,:); %link one absolute angle
dth1 = z(2,:); %link one angular rate
th2 = z(3,:); %link two absolute angle
dth2 = z(4,:); %link two angular rate

u1 = u(1,:); %torque acting on link 1 wrt ground
u2 = u(2,:); %torque acting on link 2 wrt ground

f1 = u1 - m2.*(d2.*dth2.^2.*cos(th2) + dth1.^2.*l1.*cos(th1)).*(d2.*sin(th2) + l1.*sin(th1)) + m2.*(d2.*dth2.^2.*sin(th2) + dth1.^2.*l1.*sin(th1)).*(d2.*cos(th2) + l1.*cos(th1)) - g.*m2.*(d2.*cos(th2) + l1.*cos(th1)) - d1.*g.*m1.*cos(th1);
f2 = u2 - d2.*g.*m2.*cos(th2) + d2.*dth1.^2.*l1.*m2.*sin(th1 - th2);

M11 = - I1 - d1.^2.*m1.*cos(th1).^2 - d1.^2.*m1.*sin(th1).^2 - l1.*m2.*cos(th1).*(d2.*cos(th2) + l1.*cos(th1)) - l1.*m2.*sin(th1).*(d2.*sin(th2) + l1.*sin(th1));
M12 = - I2 - d2.*m2.*cos(th2).*(d2.*cos(th2) + l1.*cos(th1)) - d2.*m2.*sin(th2).*(d2.*sin(th2) + l1.*sin(th1));
M21 = -d2.*l1.*m2.*cos(th1 - th2);
M22 = - I2 - d2.^2.*m2;

D = M11.*M22 - M12.*M21;

ddth1 = (f2.*M12 - f1.*M22)./D;
ddth2 = -(f2.*M11 - f1.*M21)./D;

dz = [...
    dth1; %derivative of link one absolute angle
    ddth1; %derivative of link one angular rate
    dth2; %derivative of link two absolute angle
    ddth2; %derivative of link two angular rate
];
end 
