%
% Problem 1b: Lyapunov-based Parameter Estimation
%
clear

% True system parameters
m_true = 1.315;
b_true = 0.225;
k_true = 0.725;

% Simulation parameters
Ts = 0.001;
T_total = 40;
t = 0:Ts:T_total;
N = length(t);

% Gamma setup
gamma = 0.66;
fprintf('Using gamma = %.4f (Lyapunov Based Estimation)\n', gamma);

% Define sine input only (as per problem statement)
u = 2.5 * sin(t);

% Simulate the true system
x = zeros(1, N);
dx = zeros(1, N);
ddx = zeros(1, N);
x(1) = 0; dx(1) = 0;
for k = 1:N-1
    f = @(x_, dx_, u_) (1/m_true) * (u_ - b_true * dx_ - k_true * x_);
    k1 = f(x(k), dx(k), u(k));
    k2 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k1, u(k));
    k3 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k2, u(k));
    k4 = f(x(k) + Ts * dx(k), dx(k) + Ts * k3, u(k));
    ddx(k) = k1;
    dx(k+1) = dx(k) + Ts/6 * (k1 + 2*k2 + 2*k3 + k4);
    x(k+1) = x(k) + Ts * dx(k);
end
ddx(1:end-1) = diff(dx) / Ts;
ddx(end) = ddx(end-1);

% Estimation using Lyapunov structure
phi_all = [ddx; dx; x];  % shape: [3 x N]
theta_hat = zeros(3, N);
theta_hat(:, 1) = [1; 1; 1];

for k = 1:N-1
    phi = phi_all(:,k);
    y = u(k);
    y_hat = theta_hat(:,k)' * phi;
    e = y - y_hat;
    theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi;
end

% Final estimates
fprintf('\nFinal estimates:\n');
fprintf('Estimated m: %.4f, b: %.4f, k: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end));

% Plot results
figure('Name', 'Lyapunov Estimation (notes form)', 'Position', [100, 100, 1280, 860]);
sgtitle(sprintf('Input: sine   |   Gamma = %.3f   |   Lyapunov', gamma), 'FontWeight', 'bold');

subplot(3,1,1);
plot(t, theta_hat(1,:), 'b', 'LineWidth', 1.5);
ylabel('$$\hat{m}(t)$$ [kg]', 'Interpreter', 'latex');
grid on; title('Εκτίμηση μάζας');

subplot(3,1,2);
plot(t, theta_hat(2,:), 'r', 'LineWidth', 1.5);
ylabel('$$\hat{b}(t)$$ [Ns/m]', 'Interpreter', 'latex');
grid on; title('Εκτίμηση απόσβεσης');

subplot(3,1,3);
plot(t, theta_hat(3,:), 'k', 'LineWidth', 1.5);
ylabel('$$\hat{k}(t)$$ [N/m]', 'Interpreter', 'latex');
xlabel('t [sec]');
grid on; title('Εκτίμηση ελαστικότητας');

if ~exist('output', 'dir')
    mkdir('output');
end
saveas(gcf, sprintf('output/Prob1b_lyapunov_gamma%.3f_%ds.png', gamma, T_total));

% Reconstruct estimated output x_hat(t)
x_hat = zeros(1, N);
dx_hat = zeros(1, N);
dx_hat(1) = 0;
for k = 1:N-1
    m_hat = theta_hat(1,k);
b_hat = theta_hat(2,k);
k_hat = theta_hat(3,k);
ddx_hat = (u(k) - b_hat * dx_hat(k) - k_hat * x_hat(k)) / m_hat;
    dx_hat(k+1) = dx_hat(k) + Ts * ddx_hat;
    x_hat(k+1) = x_hat(k) + Ts * dx_hat(k);
end
e_x = x - x_hat;

% Plot extra figure with x, x_hat and e_x
figure('Name', 'System Response vs Estimation', 'Position', [100, 100, 1280, 860]);
sgtitle(sprintf('System Response and Error | Gamma = %.3f', gamma), 'FontWeight', 'bold');

subplot(2,1,1);
plot(t, x, 'b', t, x_hat, '--r', 'LineWidth', 1.5);
legend('x(t)', 'x_{hat}(t)', 'Location', 'Best');
ylabel('Θέση [m]');
grid on; title('Αντίδραση Συστήματος και Εκτίμηση');

subplot(2,1,2);
plot(t, e_x, 'k', 'LineWidth', 1.5);
ylabel('e_x(t)');
grid on; title('Σφάλμα θέσης: x(t) - x_{hat}(t)');



saveas(gcf, sprintf('output/Prob1b_extrastates_gamma%.3f_%ds.png', gamma, T_total));