% === Problem 3b: Effect of Sampling Period Ts on Estimation Accuracy ===

clear; close all;

% True 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];

% Parameters of input
A0 = 4;
omega = 2;

% Time settings
T_final = 20;
dt = 1e-4;
t_full = 0:dt:T_final;

% Simulate system with very fine resolution
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);

% Sampling periods to test
Ts_list = [0.01, 0.05, 0.1, 0.2, 0.5];
n_cases = length(Ts_list);
rel_errors_all = zeros(3, n_cases);

for i = 1:n_cases
    Ts = Ts_list(i);

    % Resample at this 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);

    % Compute dq and ddq with central differences
    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;        % Truncate to valid range (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('Ts = %.3f s → mL^2=%.4f (%.2f%%), c=%.4f (%.2f%%), mgL=%.4f (%.2f%%)\n', ...
        Ts, theta_hat(1), rel_error(1), ...
        theta_hat(2), rel_error(2), ...
        theta_hat(3), rel_error(3));
end

% === Plot ===
figure('Name', 'Problem 3b - Effect of Sampling Period', 'Position', [100, 100, 1000, 600]);
plot(Ts_list, rel_errors_all', '-o', 'LineWidth', 2, 'MarkerSize', 4);
legend({'mL^2', 'c', 'mgL'}, 'Location', 'northwest');
xlabel('Sampling Period Ts [sec]');
ylabel('Relative Error [%]');
title('Effect of Ts on Parameter Estimation');
grid on;

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