function totalenergy = energy(b)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% totalenergy = energy(b)                                %
%  -- Calculation of energy consumption in moving        %
%      the pendulum --                                   %
%                                                        %
% energy.m computes the total required energy to drive   %
% the position of pendulum from theta(0)                 %
% to its final state.                                    %
%                                                        %
% input  b  Real control parameter                       %
% output totalenergy     Real total required energy      %
%                                                        %
%  Dianne P. O'Leary and Yalin E. Sadguyu                %
%  12/02 and 01/03                                       %
%                                                        %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

global mydata mypenalty

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the initial conditions (theta(0), theta'(0)).      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

ic = [pi/4 0];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the times for determining the solution.            %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

h = 0.01;
tc = 5;
t = [0:h:tc+5];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Pass the control parameters to the ODE and solve it.   %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mydata.Bc = b;

[t,y] = ode45('pendulum',t,ic); 
itc = find(t==tc);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Estimate the total energy using the rectangle rule of  %
% integration, adding a penalty term if the system       %
% failed to reach its target within tc seconds.          %
%                                                        %
% The penalty term is related to the angular difference  %
% between the system's position after time tc and the    %
% desired position.                                      %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

mypenalty = max(abs(y(itc:end,1)-mydata.forceangle));

if (mypenalty < 1.e-3)
   mypenalty = 0;
end

u = mydata.m * mydata.g * sin(mydata.forceangle);


totalenergy = h * mydata.m * mydata.l * sum(abs ...
               (mydata.Bc*y(1:itc,2))) + abs(u)*tc + 1.e6*mypenalty;