% === Problem 2: Parameter Estimation using Least Squares ===

% True data for comparison
m = 0.75;
L = 1.25;
c_true = 0.15;
g = 9.81;

mL2_true = m * L^2;
mgL_true = m * g * L;

theta_true = [mL2_true; c_true; mgL_true];

% Load sampled data from Problem 1
data = readtable('output/problem1_data.csv');
t = data.t;
q = data.q;
dq = data.dq;
ddq = data.ddq;
u = data.u;

% Build regression matrix X and target vector y = u
X = [ddq, dq, q];  % columns correspond to coefficients of [mL^2, c, mgL]
y = u;

% Least Squares estimation: theta_hat = [mL^2; c; mgL]
theta_hat = (X' * X) \ (X' * y);

% Extract individual parameters (optional interpretation)
mL2_est = theta_hat(1);
c_est   = theta_hat(2);
mgL_est = theta_hat(3);

% Reconstruct ddq_hat using the estimated parameters
ddq_hat = (1 / mL2_est) * (u - c_est * dq - mgL_est * q);

% Numerical integration to recover dq̂(t) and q̂(t)
Ts = t(2) - t(1);
N = length(t);
dq_hat = zeros(N,1);
q_hat = zeros(N,1);

% Initial conditions
dq_hat(1) = dq(1);
q_hat(1) = q(1);

for k = 2:N
    dq_hat(k) = dq_hat(k-1) + Ts * ddq_hat(k-1);
    q_hat(k) = q_hat(k-1) + Ts * dq_hat(k-1);
end

% Estimation error
e_q = q - q_hat;

% === Plots ===
figure('Name', 'Problem 2 - LS Estimation', 'Position', [100, 100, 1280, 800]);

subplot(3,1,1);
plot(t, q, 'b', t, q_hat, 'r--');
legend('q(t)', 'q̂(t)');
title('Actual vs Estimated Angle');
ylabel('Angle [rad]');
grid on;

subplot(3,1,2);
plot(t, e_q, 'k');
title('Estimation Error e_q(t) = q(t) - q̂(t)');
ylabel('Error [rad]');
grid on;

subplot(3,1,3);
bar(["mL^2", "c", "mgL"], theta_hat);
title('Estimated Parameters');
ylabel('Value');
grid on;
saveas(gcf, 'output/Prob2_20s_Ts0.1.png');

fprintf('   Actual Parameters:  mL^2=%f, c=%f, mgL=%f\n', theta_true(1), theta_true(2), theta_true(3));
fprintf('Estimated Parameters:  mL^2=%f, c=%f, mgL=%f\n', theta_hat(1), theta_hat(2), theta_hat(3));