% === Problem 3c: Effect of Input Amplitude A0 on Estimation Accuracy ===

clear; close all;

% True system parameters
m = 0.75;
L = 1.25;
c = 0.15;
g = 9.81;

mL2_true = m * L^2;
mgL_true = m * g * L;
theta_true = [mL2_true; c; mgL_true];

% Simulation settings
omega = 2;       % input frequency
Ts = 0.1;        % sampling period
dt = 1e-4;       % integration resolution
T_final = 20;    % simulation time

% Amplitudes to test
A0_list = [1, 2, 4, 6, 8, 16];
n_cases = length(A0_list);
rel_errors_all = zeros(3, n_cases);

for i = 1:n_cases
    A0 = A0_list(i);

    % Simulate system
    t_full = 0:dt:T_final;
    odefun = @(t, x) [
        x(2);
        (1/(m*L^2)) * (A0*sin(omega*t) - c*x(2) - m*g*L*x(1))
    ];
    x0 = [0; 0];
    [t_sim, x_sim] = ode45(odefun, t_full, x0);

    % Resample at Ts
    t_sampled = t_sim(1):Ts:t_sim(end);
    q = interp1(t_sim, x_sim(:,1), t_sampled);
    u = A0 * sin(omega * t_sampled);
    N = length(t_sampled);

    % Estimate derivatives
    dq = zeros(N,1);
    ddq = zeros(N,1);
    for k = 2:N-1
        dq(k) = (q(k+1) - q(k-1)) / (2*Ts);
        ddq(k) = (q(k+1) - 2*q(k) + q(k-1)) / Ts^2;
    end

    % LS Estimation
    idx = 2:N-1;
    X = [ddq(idx), dq(idx), q(idx)'];
    y = u(idx).';
    theta_hat = (X' * X) \ (X' * y);

    rel_error = abs((theta_hat - theta_true) ./ theta_true) * 100;
    rel_errors_all(:, i) = rel_error;

    % Print
    fprintf('A0 = %d → mL^2=%.4f (%.2f%%), c=%.4f (%.2f%%), mgL=%.4f (%.2f%%)\n', ...
        A0, theta_hat(1), rel_error(1), ...
        theta_hat(2), rel_error(2), ...
        theta_hat(3), rel_error(3));
end

% === Plot ===
figure('Name', 'Problem 3c - Effect of A0', 'Position', [100, 100, 1000, 600]);
plot(A0_list, rel_errors_all', '-o', 'LineWidth', 2, 'MarkerSize', 4);
legend({'mL^2', 'c', 'mgL'}, 'Location', 'northeast');
xlabel('Input Amplitude A_0');
ylabel('Relative Error [%]');
title('Effect of Input Amplitude on Parameter Estimation');
grid on;
ylim([0 1.1]);

saveas(gcf, 'output/Prob3c_AmplitudeEffect.png');