%
% Problem 2c: Estimation under external disturbance d(t)
%
clear

% True system parameters
a1 = 2.0;
a2 = 1.0;
a3 = 0.5;
b = 2.0;

% Simulation setup
Ts = 0.001;
T_total = 30;
t = 0:Ts:T_total;
N = length(t);

% Reference trajectory
r_d = zeros(1, N);
r_d(t >= 10 & t < 20) = pi/10;

% Smooth bound phi(t)
phi0 = 1.5;
phi_inf = 0.05;
lambda = 0.5;
phi = (phi0 - phi_inf) * exp(-lambda * t) + phi_inf;

% Control parameters
k1 = 1.0;
k2 = 1.0;
rho = 1.0;

% External disturbance
d = 0.15 * sin(0.5 * t);

% Initial conditions
r = zeros(1, N);
dr = zeros(1, N);
ddr = zeros(1, N);

% Estimated trajectory reconstruction
r_hat = zeros(1, N);
dr_hat = zeros(1, N);
dr_hat(1) = 0; r_hat(1) = 0;

% Parameter estimation setup
theta_hat = zeros(4, N);
theta_hat(:,1) = [1; 1; 1; 1];
gamma = 0.66;

alpha = zeros(1, N);
u = zeros(1, N);

for k = 1:N-1
    % Control law
    z1 = (r(k) - r_d(k)) / phi(k);
    z1 = max(min(z1, 0.999), -0.999);
    alpha(k) = -k1 * log((1 + z1) / (1 - z1));

    z2 = (dr(k) - alpha(k)) / rho;
    z2 = max(min(z2, 0.999), -0.999);
    u(k) = -k2 * log((1 + z2) / (1 - z2));

    % System dynamics (with disturbance)
    phi_vec = [-dr(k); -sin(r(k)); dr(k)^2 * sin(2*r(k)); u(k)];
    ddr(k) = a1 * phi_vec(1) + a2 * phi_vec(2) + a3 * phi_vec(3) + b * phi_vec(4) + d(k);
    dr(k+1) = dr(k) + Ts * ddr(k);
    r(k+1) = r(k) + Ts * dr(k);

    % Parameter estimation (disturbance unmodeled)
    y = ddr(k);
    y_hat = theta_hat(:,k)' * phi_vec;
    e = y - y_hat;
    theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi_vec;

    % Reconstruct r_hat from estimated theta
    phi_hat = [-dr_hat(k); -sin(r_hat(k)); dr_hat(k)^2 * sin(2 * r_hat(k)); u(k)];
    dd_r_hat = theta_hat(:,k)' * phi_hat;
    dr_hat(k+1) = dr_hat(k) + Ts * dd_r_hat;
    r_hat(k+1) = r_hat(k) + Ts * dr_hat(k);
end

fprintf('\n2c: Final estimated parameters (with disturbance):\n');
fprintf('a1: %.4f, a2: %.4f, a3: %.4f, b: %.4f\n', theta_hat(1,end), theta_hat(2,end), theta_hat(3,end), theta_hat(4,end));

% Plot
figure('Name', 'Problem 2c - Estimation under Disturbance', 'Position', [100, 100, 1280, 860]);
sgtitle('Problem 2c - Estimation under External Disturbance');

subplot(3,1,1);
plot(t, theta_hat', 'LineWidth', 1.4);
legend('a_1', 'a_2', 'a_3', 'b');
ylabel('\theta estimates'); grid on; title('Εκτιμήσεις παραμέτρων');

subplot(3,1,2);
plot(t, r, 'b', t, r_hat, '--r', 'LineWidth', 1.4);
legend('r(t)', 'r_{hat}(t)');
ylabel('Roll angle [rad]'); title('Πραγματικό vs εκτιμώμενο r(t)'); grid on;

subplot(3,1,3);
plot(t, r - r_hat, 'k', 'LineWidth', 1.4);
ylabel('e_r(t)'); xlabel('Time [s]'); title('Σφάλμα θέσης: r(t) - r̂(t)'); grid on;

if ~exist('output', 'dir')
    mkdir('output');
end
saveas(gcf, 'output/Problem2c_estimation.png');