% === Problem 2b: Estimation using only q(t) and u(t) ===

% True parameter values
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
data = readtable('output/problem1_data.csv');
t = data.t;
q = data.q;
u = data.u;

Ts = t(2) - t(1);
N = length(t);

% Compute dq and ddq using 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

% Build regression matrix and output vector
X = [ddq, dq, q];
y = u;

% Least Squares estimation
theta_hat = (X' * X) \ (X' * y);

% Parameter extraction
mL2_est = theta_hat(1);
c_est   = theta_hat(2);
mgL_est = theta_hat(3);

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

% Integrate to get dq_hat and q_hat
dq_hat = zeros(N,1);
q_hat = zeros(N,1);
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 2b - LS Estimation from q only', '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 from q(t) only');
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;

% Save figure
saveas(gcf, 'output/Prob2b_20s_Ts0.1.png');

% Print results
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));