function [energy, kinetic, potential] = energy_3_link(z,P) 
%[ENERGY, KINETIC, POTENTIAL] = ENERGY_3_LINK(Z,P)
% 
%FUNCTION:  This function computes the energy of a double
%    pendulum.
%INPUTS: 
%
%
%OUTPUTS: 
%    energy = [1 X nTime] vector of total energy
%    kinetic = [1 X nTime] vector of kinetic energy
%    potential = [1 X nTime] vector of potential energy
% 
%NOTES:
%    This file was automatically generated by writeEnergy.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

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

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

kinetic = (I1.*dth1.^2)./2 + (I2.*dth2.^2)./2 + (I3.*dth3.^2)./2 + (m1.*(d1.^2.*dth1.^2.*cos(th1).^2 + d1.^2.*dth1.^2.*sin(th1).^2))./2 + (m2.*((dth1.*l1.*cos(th1) + d2.*dth2.*cos(th2)).^2 + (d2.*dth2.*sin(th2) + dth1.*l1.*sin(th1)).^2))./2 + (m3.*((dth1.*l1.*cos(th1) + dth2.*l2.*cos(th2) + d3.*dth3.*cos(th3)).^2 + (d3.*dth3.*sin(th3) + dth1.*l1.*sin(th1) + dth2.*l2.*sin(th2)).^2))./2;
potential = g.*m3.*(d3.*sin(th3) + l1.*sin(th1) + l2.*sin(th2)) + g.*m2.*(d2.*sin(th2) + l1.*sin(th1)) + d1.*g.*m1.*sin(th1);
energy = potential + kinetic;

end 
