function double_pendulum(plotCM)
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Simulation of the Equations of Motion for a Planar Double
    % Pendulum (with zero initial velocity conditions)
    %
    % By: Alyssa Novelia (4/10/2014)
    % Revised: Daniel Kawano (12/18/2017)
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %
    % Nested function layout:
    %   start dragging mass 1
    %       drag mass 1
    %   stop dragging mass 1
    %       stop executing drag command
    %       start dragging mass 2
    %           drag mass 2
    %       stop dragging mass 2
    %           stop executing drag command 
    %           get initial conditions
    %           start animating motion
    %               integrate EOM according to initial conditions
    %           animate motion!
    %
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    % plotCM is a variable with a value of 0 or 1 that specifies whether to 
    % plot the animation side-by-side with the motion of the representative 
    % particle in the configuration space; 0 = no plot, 1 = include plot. 
    % If no input argument is specified, then plotCM = 1 by default.
    if nargin == 0
        plotCM = 1;
    end    
    
    % m1 = upper hanging mass
    % m2 = lower hanging mass
    % L1 = rod connecting the origin to m1
    % L2 = rod connecting m1 to m2
    
    % Main parameters:
    lambda = 1;             % rod length ratio, L2/L1
    mu = 1;                 % mass ratio, m2/m1
    
    % Animation is plotted faster when using fewer timesteps -- higher
    % animation speed, lower plotting frequency:
    animation_speed = 8;   
    
    % Figure objects' properties:
    L1_length = 1;                          % length of L1 in MATLAB figure units
    L2_length = L1_length*lambda;
    max_xy = L1_length + L2_length + 0.5;   % for XLim and YLim
    
    % Draw and set parameters of main figure:
    main_fig = figure;
    aH = axes('XLim', [-max_xy max_xy], 'YLim', [-max_xy max_xy]);
    axis equal;
    set(main_fig, 'Color', 'White', 'Name', 'Double pendulum', 'Position', [375 198 622 388]);
    title('Click and drag the upper hanging mass');
    hold on;

    % Draw objects:
    origin = line('xdata', 0, 'ydata', 0, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 1);
    L1 = line('xdata', [0 0], 'ydata', [0 -L1_length], 'LineWidth', 2, 'Color', 'k');
    L2 = line('xdata', [0 0], 'ydata', [-L1_length -(L1_length + L2_length)], 'LineWidth', 2, 'Color', 'k');
    mass2 = line('xdata', 0, 'ydata', -(L1_length + L2_length), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3);
    mass1 = line('xdata', 0, 'ydata', -L1_length, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3);
    
    % Start mass1 interaction:
    set(mass1, 'ButtonDownFcn', @StartDragFcn1);        % enable clicking mass1
   
    % Call StartDragFcn1 when mass1 is clicked:
    set(main_fig, 'WindowButtonUpFcn', @StopDragFcn1);  % execute this line when mouse button is released

    function StartDragFcn1(varargin)
        % Call dragging function when mass1 is dragged:
        set(main_fig, 'WindowButtonMotionFcn', @DraggingFcn1);
    end

    function DraggingFcn1(varargin)  

        % Dragging mass1:
        set(mass1, 'Color', 'r');               % change mass1 color when object is dragged
        cursor_pt = get(aH, 'CurrentPoint');    % get real-time cursor position

        theta1_t0 = atan(cursor_pt(1,2)/cursor_pt(1,1));    % theta defined by MATLAB, not by coordinate system defined in notes (atan(Y/X))
        if cursor_pt(1,1) < 0
            theta1_t0 = pi + theta1_t0;         % differentiate 2nd quadrant from 4th, 3rd from 1st
        end
        
        % Current masses' x and y coordinates:
        mass1_pt = L1_length*[cos(theta1_t0) sin(theta1_t0)];
        mass2_pt = mass1_pt + L2_length*[0 -1]; 	% mass2 relative to mass 1 does not update
        
        % Update all objects' positions when dragged:
        set(mass1, 'xdata', mass1_pt(1,1), 'ydata', mass1_pt(1,2));
        set(L1, 'xdata', [0 mass1_pt(1,1)], 'ydata', [0 mass1_pt(1,2)]);
        set(mass2, 'xdata', mass2_pt(1,1), 'ydata', mass2_pt(1,2));
        set(L2, 'xdata', [mass1_pt(1,1) mass2_pt(1,1)], 'ydata', [mass1_pt(1,2) mass2_pt(1,2)]);  
        
    end
    
    function StopDragFcn1(varargin)
        
        mass1_t0 = [get(mass1, 'xdata') get(mass1, 'ydata')];   % get mass1 position when released, use as initial condition
        set(mass1, 'Color', 'k');
        set(main_fig, 'WindowButtonMotionFcn', '');         % disable dragging function when mouse button is released (IMPORTANT)
       
        title('Click and drag the lower hanging mass');     % update instruction, drag mass2 next
        
        % Start mass 2 interaction:
        set(mass2, 'ButtonDownFcn', @StartDragFcn2);        % enable clicking mass2
        
        % Call StartDragFcn1 when mass2 is clicked:
        set(main_fig, 'WindowButtonUpFcn', @StopDragFcn2);  % execute this line when mouse button is released
        
        function StartDragFcn2(varargin)
            % Call dragging function when mass2 is dragged:
            set(main_fig, 'WindowButtonMotionFcn', @DraggingFcn2);
        end
        
        function DraggingFcn2(varargin)  
        
            % Dragging mass 2:
            set(mass2, 'Color', 'r');                       % change mass2 color when object is dragged
            cursor_pt = get(aH, 'CurrentPoint');            % get real-time cursor position
            cursor_rel_pt = cursor_pt(1,1:2) - mass1_t0;    % position of cursor relative to m1

            theta2_t0 = atan(cursor_rel_pt(1,2)/cursor_rel_pt(1,1));    % theta defined by MATLAB, not by coordinate system defined in notes
            if cursor_rel_pt(1,1) < 0
                theta2_t0 = pi + theta2_t0;                             % differentiate 2nd quadrant from 4th, 3rd from 1st
            end

            % Calculate only mass2's x and y coordinates:
            mass2_pt = mass1_t0 + L2_length*[cos(theta2_t0) sin(theta2_t0)];

            % Update mass2's and L2's positions when mass2 is dragged:
            set(mass2, 'xdata', mass2_pt(1,1), 'ydata', mass2_pt(1,2));
            set(L2, 'xdata', [mass1_t0(1,1) mass2_pt(1,1)], 'ydata', [mass1_t0(1,2) mass2_pt(1,2)]);  

        end
        
        function StopDragFcn2(varargin)
            mass2_t0 = [get(mass2, 'xdata') get(mass2, 'ydata')];   % get mass2 position when released, use as initial condition
            set(mass2, 'Color', 'k');
            set(main_fig, 'WindowButtonMotionFcn', '');             % disable dragging function when mouse button is released (IMPORTANT)
            
            % Since our EOM is a function of theta1 and theta2, calculate
            % those values from positions of the masses.
            
            % Calculate initial theta1:
            theta1_t0_MAT = atan(mass1_t0(1,2)/mass1_t0(1,1)); 
            if mass1_t0(1,1) < 0
                theta1_t0_MAT = pi + theta1_t0_MAT;     % differentiate 2nd quadrant from 4th, 3rd from 1st
            end
            
            % Calculate initial theta2. Note how theta2 is defined (angle of
            % L2 from the horizontal (MATLAB figure), or vertical (EOM)):
            mass2rel_t0 = mass2_t0 - mass1_t0;
            theta2_t0_MAT = atan(mass2rel_t0(1,2)/mass2rel_t0(1,1));
            if mass2rel_t0(1,1) < 0
                theta2_t0_MAT = pi + theta2_t0_MAT;     % differentiate 2nd quadrant from 4th, 3rd from 1st
            end
            
            % Rotate MATLAB basis 90 deg CW to get EOM basis:
            theta1_t0_EOM = theta1_t0_MAT + pi/2;
            theta2_t0_EOM = theta2_t0_MAT + pi/2;            
           
            % Create new subplot (for pendulum):
            if plotCM
                left_fig = subplot(1,2,1);
            else
                clf;
                left_fig = subplot(1,1,1);
            end
            title('Planar double pendulum');
            axis equal;
            set(left_fig, 'XLim', [-max_xy max_xy], 'YLim', [-max_xy max_xy]);
            
            % Duplicate objects on left figure:
            Lorigin = line('xdata', 0, 'ydata', 0, 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 1);
            Lmass2 = line('xdata', mass2_t0(1,1), 'ydata', mass2_t0(1,2), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3);
            Lmass1 = line('xdata', mass1_t0(1,1), 'ydata', mass1_t0(1,2), 'Marker', 'o', 'MarkerFaceColor', 'k', 'Color', 'k', 'LineWidth', 3);
            LL1 = line('xdata', [0 mass1_t0(1,1)], 'ydata', [0 mass1_t0(1,2)], 'LineWidth', 2, 'Color', 'k');
            LL2 = line('xdata', [mass1_t0(1,1) mass2_t0(1,1)], 'ydata', [mass1_t0(1,2) mass2_t0(1,2)], 'LineWidth', 2, 'Color', 'k');
            Lpathline1 = line('xdata', 0, 'ydata', 0, 'LineWidth', 1, 'Color', 'r', 'LineStyle', '-');
            Lpathline2 = line('xdata', 0, 'ydata', 0, 'LineWidth', 1, 'Color', 'b', 'LineStyle', '-');
            
            if plotCM == 1
                % Create new subplot (for configuration manifold):
                right_fig = subplot(1,2,2); title({'Representative particle''s trajectory'; 'on the configuration manifold'});
                axis equal;
                set(right_fig, 'XLim', [-2 2], 'YLim', [-2 2], 'ZLim', [-2 2], ...
                    'CameraPosition', [-2 -2 2]);
                CM_path = line('xdata', 0, 'ydata', 0, 'zdata', 0, 'LineWidth', 2, 'Color', 'b');
                CM_pointer = line('xdata', 0, 'ydata', 0, 'zdata', 0, 'LineWidth', 3, 'MarkerFaceColor', 'b', 'Marker', '.');
                hold on;

                % Define torus radius:
                R2 = 0.5;   
                R1 = 1.5;

                % Credits to
                % http://www.cise.ufl.edu/research/sparse/MATLAB/6thEdition/torus.m
                % for the torus code:
                [x y z] = torus(R2, 30, R1);
                torus_obj = surf(x,y,z, 'EdgeColor', [1 1 1]); alpha(0.1);
            end
            
            % Run plotting function:
            plotdoublependulum(theta1_t0_EOM, theta2_t0_EOM);
            
            
            function plotdoublependulum(theta1, theta2)
                % Set ode45 tolerance:
                tol = 1e-8;
                options = odeset('abstol', tol, 'reltol', tol);

                % Run ode45. 
                % Y = [theta1, theta2, theta1', theta2']:
                [T Y] = ode45(@doublependulumEOM, [0 30], [theta1 theta2 0 0], options);
              
                % LEFT SUBPLOT
                
                % Bound the angles from -pi to pi:
                theta1_EOM = normalize_theta(Y(:,1));
                theta2_EOM = normalize_theta(Y(:,2));
                               
                % Calculate the masses' positions (in MATLAB figure) over
                % time from the angles defined by EOM.
                %
                % Rotate e_r_EOM 90 deg CCW:
                %
                % [ 0  1]* [cos(theta_EOM)]  = [sin(theta_EOM) ]
                % [-1  0]  [sin(theta_EOM)]    [-cos(theta_EOM)]
                
                mass1_MAT = L1_length*[sin(theta1_EOM) -cos(theta1_EOM)];
                mass2_MAT = mass1_MAT + L2_length*[sin(theta2_EOM) -cos(theta2_EOM)];
                               
                % RIGHT SUBPLOT
                
                % Config space position: r = R1(cos(x)E1 + sin(x)E2) +
                % R2(cos(y)E2 + sin(y)E3):
                
                if plotCM
                    rx = R1*cos(theta1_EOM) + R2*cos(theta2_EOM).*cos(theta1_EOM);
                    ry = R1*sin(theta1_EOM) + R2*cos(theta2_EOM).*sin(theta1_EOM);
                    rz = R2*sin(theta2_EOM);
                end

                % Draw the animation sequence:
                for iii = 1:animation_speed:size(Y,1)
                   
                    % Left figure (real space):
                    set(Lpathline1, 'xdata', mass1_MAT(1:iii), 'ydata', mass1_MAT(1:iii,2));
                    set(LL1, 'xdata', [0 mass1_MAT(iii,1)], 'ydata', [0 mass1_MAT(iii,2)]);
                    set(Lmass1, 'xdata', mass1_MAT(iii,1), 'ydata', mass1_MAT(iii,2));

                    set(Lpathline2, 'xdata', mass2_MAT(1:iii), 'ydata', mass2_MAT(1:iii,2));
                    set(LL2, 'xdata', [mass1_MAT(iii,1) mass2_MAT(iii,1)], 'ydata', [mass1_MAT(iii,2) mass2_MAT(iii,2)]);  
                    set(Lmass2, 'xdata', mass2_MAT(iii,1), 'ydata', mass2_MAT(iii,2));
                              
                    % Right figure (configuration space):
                    if plotCM
                        set(CM_path, 'xdata', rx(1:iii), 'ydata', ry(1:iii), 'zdata', rz(1:iii));
                        set(CM_pointer, 'xdata', rx(iii), 'ydata', ry(iii), 'zdata', rz(iii));
                    end
                    drawnow;
        
                end
 
                % Create phase portrait for theta1:
                pplane_theta = figure;
                set(pplane_theta, 'Color', 'White', 'Name', 'Phase portaits', 'Position', [375 198 622 388]);
                
                xmax = max(max(theta1_EOM, theta2_EOM));
                xmin = min(min(theta1_EOM, theta2_EOM));
                ymax = max(max(Y(:,3), Y(:,4)));
                ymin = min(min(Y(:,3), Y(:,4)));
                
                pplane_theta1 = subplot(1,2,1);
                plot(theta1_EOM, Y(:,3), 'r', 'linewidth', 2);
                set(pplane_theta1, 'Color', 'White');
                xlabel('\theta_1'); 
                ylabel('\theta_1^{\prime}   ', 'rotation', 0);
                axis([xmin, xmax, ymin, ymax]);
                axis equal; grid on;
                               
                % Create phase portrait for theta2:
                pplane_theta2 = subplot(1,2,2);
                plot(theta2_EOM, Y(:,4), '-b', 'linewidth', 2);
                set(pplane_theta2, 'Color', 'White');
                xlabel('\theta_2'); 
                ylabel('\theta_2^{\prime}   ', 'rotation', 0);
                axis([xmin, xmax, ymin, ymax]);
                axis equal; grid on;
                
                % Plot x, y trajectory over time:
                trajectory = figure;
                set(trajectory, 'Color', 'White', 'Name', 'Trajectory of the masses', 'Position', [375 198 622 388]);
                
                % Mass 1 trajectory:
                subplot(2,1,1);
                plot(T, mass1_MAT(:,1), 'k', T, mass1_MAT(:,2), 'b', 'linewidth', 2);
                xlabel('Time, \tau'); ylabel('Displacement')
                legend('\itx\rm(\tau)', '\ity\rm(\tau)');
                title('Displacement of mass \itm\rm_1'); grid on;
                      
                % Mass 2 trajectory:
                subplot(2,1,2)
                plot(T, mass2_MAT(:,1), 'k', T, mass2_MAT(:,2), 'b', 'linewidth', 2);
                xlabel('Time, \tau'); ylabel('Displacement')
                legend('\itx\rm(\tau)', '\ity\rm(\tau)');
                title('Displacement of mass \itm\rm_2'); grid on;

            end
            
            function dY = doublependulumEOM(T, Y)
                % Y = [theta1, theta2, theta1', theta2'];
                theta1 = Y(1);
                theta2 = Y(2);
                dtheta1 = Y(3);
                dtheta2 = Y(4);
                
                dY = zeros(4,1);
                
                % Pre-calculate M and f:
                M = [1+mu, mu*lambda*cos(theta1-theta2);
                     mu*lambda*cos(theta1-theta2), mu*lambda^2];
                f = [-(1+mu)*sin(theta1) - mu*lambda*(sin(theta1-theta2))*dtheta2^2;
                     mu*lambda*(-sin(theta2) + (sin(theta1-theta2))*dtheta1^2)];
                
                % Equations of motion for planar double pendulum in state
                % space:
                dY = [dtheta1;
                      dtheta2;
                      M\f];
            end
        end   

    end       
end