function EnergyPlot(data,par)
% EnergyPlot  Plots the kinetic and potential energy as function of time.
%   Call Animation(data,par) after Step or Walk to display the energy plot.
%
%   See also: Step, Walk, Animation
 
% Daniël Karssen and Martijn Wisse
% Delft Biorobotics Lab, TUDelft, March 2008
% created for tutorial of Dynamicwalking 2008

% initialize variables
I=size(data.s,1);
Ekin = NaN*ones(I,1);
Epot = NaN*ones(I,1);

for i = 1:I
    % unpack the state vector
    phi_st = data.s(i,1);
    phi_sw = data.s(i,2);
    phi_st_d = data.s(i,3);
    phi_sw_d = data.s(i,4);
    
    % calculate the position of the CoMs
    pos_h = [-par.R*phi_st - (par.L-par.R)*sin(phi_st)
             par.R         + (par.L-par.R)*cos(phi_st)];
    pos_h = pos_h + [data.posfoot(i);0];
    x = [ pos_h + rotationmatrix(phi_st)*[par.B; -par.C]    % x- and y position of CoM of the stance leg 
          phi_st                                            % angle of CoM of the stance leg 
          pos_h + rotationmatrix(phi_sw)*[par.B; -par.C]    % x- and y position of CoM of the swing leg 
          phi_sw                                ];          % angle of CoM of the swing leg    
    
    % calculate the velocities of the CoMs
    T = [...
    -par.R-(par.L-par.R)*cos(phi_st)-sin(phi_st)*par.B+cos(phi_st)*par.C,                                   0;
          -(par.L-par.R)*sin(phi_st)+cos(phi_st)*par.B+sin(phi_st)*par.C,                                   0;
                                                                       1,                                   0;
                                        -par.R-(par.L-par.R)*cos(phi_st),-sin(phi_sw)*par.B+cos(phi_sw)*par.C;
                                              -(par.L-par.R)*sin(phi_st), cos(phi_sw)*par.B+sin(phi_sw)*par.C;
                                                                       0,                                   1];
    xd = T*[phi_st_d;phi_sw_d];
    
    % mass matrix
    M = diag([par.m, par.m, par.I, par.m, par.m, par.I]);
    
    % gravity vector
    f = [sin(par.gamma);-cos(par.gamma);0;sin(par.gamma);-cos(par.gamma);0];
    
    % kinetic energy
    Ekin(i) = 0.5*xd'*M*xd;
    
    % potential energy
    Epot(i) = -x'*M*f*par.g;
end

% zero potential energy with the first position
Epot=Epot-Epot(1);

% plot figure
figure
hold on
plot(data.t,Ekin,'r')
plot(data.t,Epot,'b')
plot(data.t,Ekin+Epot,'k')
legend('Ekin','Epot','Etot','Location','Best')