% Nonlinear Pendulum Simulation 
clear; clc; close all;

% Parameters
g = 9.81;          % Gravity (m/s^2)
L = 1.0;           % Pendulum length (m)
m = 1.0;           % Bob mass (kg)
b = 0.05;          % Damping coefficient (N.m.s)
dt = 0.016;        % Time step (s) ~ 60 fps
t_end = 30;        % Total simulation time (s)

% Initial State
theta = deg2rad(60);   % Angular position (rad)
omega = 0;             % Angular velocity (rad/s)
t = 0;

t_hist = t;
theta_hist = rad2deg(theta);

% Initial bob position
x = L*sin(theta);
y = -L*cos(theta);

% Figure 1: Animation
fig1 = figure('Name', 'Pendulum Animation', ...
              'Position', [100, 300, 400, 500]);

hold on;

% Support
plot([-0.25, 0.25], [0, 0], 'k-', 'LineWidth', 4);

% Pivot
plot(0, 0, 'ko', ...
    'MarkerSize', 8, ...
    'MarkerFaceColor', 'k');

% Pendulum rod
rod = plot([0, x], [0, y], ...
    'b-', 'LineWidth', 3);

% Pendulum bob
bob = plot(x, y, 'ro', ...
    'MarkerSize', 20, ...
    'MarkerFaceColor', '#e74c3c');

xlim([-1.2*L, 1.2*L]);
ylim([-1.2*L, 0.3*L]);

axis equal;
grid on;

title('Physical Simulation');
xlabel('Horizontal Position (m)');
ylabel('Vertical Position (m)');

% Figure 2: Trajectory Plot
fig2 = figure('Name', 'Angular Position Trajectory', ...
              'Position', [520, 300, 600, 500]);

trajectory_line = plot(t_hist, theta_hist, ...
    'b-', 'LineWidth', 2);

xlim([0, t_end]);
ylim([-70, 70]);

title('Angular Position vs. Time');
xlabel('Time (s)');
ylabel('Angle \theta (deg)');
grid on;


while t < t_end

    % -----------------------------------------------------
    % Flow Map (Continuous dynamics)
    % -----------------------------------------------------

    alpha = -(g/L)*sin(theta) ...
            -(b/(m*L^2))*omega;

    omega = omega + alpha*dt;
    theta = theta + omega*dt;

    % Update time
    t = t + dt;

    % -----------------------------------------------------
    % Convert angular position to Cartesian position
    % -----------------------------------------------------

    x = L*sin(theta);
    y = -L*cos(theta);

    % -----------------------------------------------------
    % Update history
    % -----------------------------------------------------

    t_hist = [t_hist, t];
    theta_hist = [theta_hist, rad2deg(theta)];

    % -----------------------------------------------------
    % Real-time Rendering Update
    % -----------------------------------------------------

    % Update rod
    set(rod, ...
        'XData', [0, x], ...
        'YData', [0, y]);

    % Update bob
    set(bob, ...
        'XData', x, ...
        'YData', y);

    % Update trajectory
    set(trajectory_line, ...
        'XData', t_hist, ...
        'YData', theta_hist);

    drawnow;       % Flush graphics immediately
    pause(dt);     % Approximately synchronize with real time

end