%
% Problem 2a: Nonlinear system roll model parameter estimation without disturbance
%
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: step profile
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;

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

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

% Output storage for control input and errors
alpha = zeros(1, N);
u = zeros(1, N);

for k = 1:N-1
    % Compute normalized errors
    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));

    % True system dynamics
    phi_true = [-dr(k); -sin(r(k)); dr(k)^2 * sin(2*r(k)); u(k)];
    ddr(k) = a1 * phi_true(1) + a2 * phi_true(2) + a3 * phi_true(3) + b * phi_true(4);

    % Integrate dynamics
    dr(k+1) = dr(k) + Ts * ddr(k);
    r(k+1) = r(k) + Ts * dr(k);

    % Estimation
    phi_est = phi_true; % same form
    y = ddr(k);
    y_hat = theta_hat(:,k)' * phi_est;
    e = y - y_hat;
    theta_hat(:,k+1) = theta_hat(:,k) + Ts * gamma * e * phi_est;
end

% Final estimates
fprintf('\n2a: Final estimated parameters:\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 parameter estimates
figure('Name', 'Problem 2a - Parameter Estimation', 'Position', [100, 100, 1280, 860]);
sgtitle('Nonlinear Roll System - Parameter Estimation');

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

subplot(2,1,2);
plot(t, r, 'b', t, r_d, '--r', 'LineWidth', 1.4);
legend('r(t)', 'r_d(t)');
ylabel('Roll angle [rad]'); xlabel('Time [s]'); grid on; title('Παρακολούθηση τροχιάς');

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