function [ tout, yout ] = pendulum(angle0, speed0, time)
% pendulum: simulate a simple pendulum.
%
%   [ tout, yout ] = pendulum(angle0, speed0, time)
%
% Simulates the standard model of a simple pendulum without any damping:
%
%      \ddot theta = -g sin \theta
%
% Input Parameters:
%
%   angle0: Double.  Initial position of the pendulum with respect to
%   hanging straight down (counter-clockwise is positive).  Optional.
%   Default = pi/4.
%
%   speed0: Double.  Initial speed of the pendulum (counter-clockwise is
%   positive).  Optional.  Default = 0.
%
%   time: Length of time for the simulation.  Default = 5.
%
% Output Parameters:
%
%   tout: Column vector.  Timesteps.
%
%   yout: Two columns, same number of rows as tout.  States at the time in
%   the corresponding row of tout.  First column is angle, second is
%   speed.
%  
% Ian Mitchell for CS 303, 3/6/08.

% Deal with the default parameter values.
if(nargin < 1)
  angle0 = pi/4;
end

if(nargin < 2)
  speed0 = 0;
end

if(nargin < 3)
  time = 5;
end
  
% What is the gravity constant?
g = 9.8;

% How long is the pendulum?  It doesn't matter to the simulation, but the
% visualization will use this information.
l = 1;

% What solver?
odefun = @ode45;              % Best first guess.
%odefun = @ode23;             % Slightly faster, significantly less accurate.
%odefun = @ode15s;            % Implicit; good guess for stiff systems. 
%odefun = @ode23t;            % Implicit but cheap, also for stiff systems.

% Plot the state vs time during execution.
figure;
options = odeset('OutputFcn', @odeplot);

% Initial condition vector.  By Matlab convention, this must be a column
% vector.
y0 = [ angle0; speed0 ];

% Integrate for the specified time.
[ tout, yout ] = feval(odefun, @pendulumModel, [ 0, time ], y0, options, g);

% We know that the first component is the angle and the second is the
% angular velocity.
title('Evolution of Pendulum State')
legend('Angle', 'Angular velocity');
xlabel('t');  ylabel('radians or radians/sec');

% Also, let's look at a phase plane plot.
figure;
plot(yout(:,1), yout(:,2), 'b*-');
title('Pendulum State in the Phase Plane')
xlabel('angle');  ylabel('angular velocity');
axis equal;

% Finally, let's look at the pendulum bob's position.
figure;
hold on;
% Initial bob position.
bob0 = [ l * sin(angle0); -l * cos(angle0) ];
% Mark the position of the pendulum's joint.
plot(0, 0, '.', 'MarkerSize', 15);
% Draw the initial state.
plot([ 0, bob0(1) ], [ 0, bob0(2) ], 'k-o', 'LineWidth', 4);
% Visualize the initial angular velocity as a line perpendicular to the
% pendulum, length proportional to the angular velocity.  We scale by 0.2
% so that this line doesn't get too large.
speed_point = bob0 + 0.2 * speed0 * [ cos(angle0); sin(angle0) ];
plot([ bob0(1), speed_point(1) ], [ bob0(2), speed_point(2) ], 'r:', ...
     'LineWidth', 2)
% Finally, plot the position of the bob for the rest of the simulation.
% We will not visualize the angular velocity at any time except the
% initial time.
plot(l*sin(yout(:,1)), -l*cos(yout(:,1)), 'b-o');
axis equal
title('Workspace Position of Pendulum Bob');
xlabel('x'), ylabel('y');


%-----------------------------------------------------------------------
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%-----------------------------------------------------------------------
function ydot = pendulumModel(t, y, g)
% pendulumModel: Example of an ODE function for a pendulum.
%
%    ydot = pendulumModel(t, y, g)
%
% Implements the standard model of a simple pendulum.  Input and output
% parameters are dictated by Matlab's ODE suite interface.  Notice that
% we have passed an additional parameter g into the model as well.
%
% Input Parameters:
%
%   t: Double.  Time of current timestep.  Ignored for this particular
%   model.
%
%   y: Two element column vector of state.  We have chosen the convention
%   that y(1) is current angle and y(2) is current angular velocity, but
%   Matlab's integrators don't care about the particular interpretation
%   of the state vector elements.
%
%   g: Double.  Gravity constant.
%
% Output Parameters:
%
%   ydot: Two element column vector of the derivative of the state.  The
%   elements are in the same order as above, so ydot(1) is the time
%   derivative of the current angle and ydot(2) is the time derivative of
%   the current angular velocity.  
  
  % Unpack the vector y.  This step is not necessary, but makes the code
  % a little more clear.
  angle = y(1);
  speed = y(2);
  
  % Construct the derivative.  Again, we normally just work with the vector
  % ydot, but for the sake of clarity I will build each component
  % separately.
  angle_dot = speed;
  speed_dot = -g * sin(angle);
  
  % Since we computed all the components separately, we need to pack them
  % back up into the output vector.
  ydot = [ angle_dot; speed_dot ];