%
% Problem 1c: Effect of bounded sinusoidal disturbance on measurement x(t)
%
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_full = 0:Ts:T_total;

% Generate full input signal
u_full = 2.5 * sin(t_full);

% Simulate the true system
x = zeros(1, length(t_full));
dx = zeros(1, length(t_full));
ddx = zeros(1, length(t_full));
x(1) = 0; dx(1) = 0;
for k = 1:length(t_full)-1
    f = @(x_, dx_, u_) (1/m_true) * (u_ - b_true * dx_ - k_true * x_);
    k1 = f(x(k), dx(k), u_full(k));
    k2 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k1, u_full(k));
    k3 = f(x(k) + Ts/2 * dx(k), dx(k) + Ts/2 * k2, u_full(k));
    k4 = f(x(k) + Ts * dx(k), dx(k) + Ts * k3, u_full(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);

% Initial estimation (clean) using Lyapunov
T_total = 40;
index_limit = round(T_total / Ts);
t = t_full(1:index_limit);
N = length(t);
u = u_full(1:index_limit);
x = x(1:index_limit);
dx = dx(1:index_limit);
ddx = ddx(1:index_limit);

phi_all = [ddx; dx; x];
theta_hat = zeros(3, N);
theta_hat(:, 1) = [1; 1; 1];
gamma = 0.66;
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

% Disturbance settings
eta0 = 0.1;
f0 = 0.5;
eta = eta0 * sin(2 * pi * f0 * t);
x_noisy = x + eta;

% Use clean derivatives, noisy position
phi_all_noise = [ddx; dx; x_noisy];
theta_hat_noise = zeros(3, N);
theta_hat_noise(:, 1) = [1; 1; 1];

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

fprintf('\n1c: Final estimates with disturbance:\n');
fprintf('Estimated m: %.4f, b: %.4f, k: %.4f\n', ...
        theta_hat_noise(1,end), theta_hat_noise(2,end), theta_hat_noise(3,end));

figure('Name', '1c - Parameter Estimation with Disturbance', 'Position', [100, 100, 1280, 860]);
sgtitle(sprintf('Lyapunov Estimation with Disturbance | η_0 = %.2f', eta0));

subplot(3,1,1);
plot(t, theta_hat(1,:), 'b', t, theta_hat_noise(1,:), '--b', 'LineWidth', 1.2);
ylabel('m(t)'); grid on; title('Μάζα');
legend('Clear', 'With noise');

subplot(3,1,2);
plot(t, theta_hat(2,:), 'r', t, theta_hat_noise(2,:), '--r', 'LineWidth', 1.2);
ylabel('b(t)'); grid on; title('Απόσβεση');
legend('Clear', 'With noise');

subplot(3,1,3);
plot(t, theta_hat(3,:), 'k', t, theta_hat_noise(3,:), '--k', 'LineWidth', 1.2);
ylabel('k(t)'); xlabel('t [s]'); grid on; title('Ελαστικότητα');
legend('Clear', 'With noise');

if ~exist('output', 'dir')
    mkdir('output');
end
saveas(gcf, sprintf('output/Prob1c_disturbance_eta%.2f.png', eta0));