%% Activity 1.1: Audit a sequential suspension workflow
% A reduced body-mode model is used to expose how a frozen spring choice
% changes the controller and actuator requirements.

clear; clc; close all;

% Fixed model and closed-loop targets
m = 300;                 % sprung mass [kg]
c = 1200;                % passive damping [N s/m]
wn_des = 6;              % desired natural frequency [rad/s]
zeta_des = 0.70;         % desired damping ratio [-]
Fmax = 2000;             % actuator-force limit [N]
k_values = (12:2:30)*1e3; % candidate spring stiffnesses [N/m]

% Half-sine force disturbance
D = 3000;                % peak disturbance [N]
Tp = 0.25;               % pulse duration [s]
t = linspace(0,3,3001).';
d = @(time) D*sin(pi*time/Tp).*(time >= 0 & time <= Tp);

n = numel(k_values);
kp = zeros(n,1); kd = zeros(n,1);
a_rms = zeros(n,1); u_peak = zeros(n,1); saturated = false(n,1);

for i = 1:n
    k = k_values(i);

    % Sequential handoff: retune the controller after k has been selected.
    kp(i) = m*wn_des^2 - k;
    kd(i) = 2*zeta_des*m*wn_des - c;

    rhs = @(time,x) [x(2); acceleration(time,x,k,kp(i),kd(i),m,c,Fmax,d)];
    [~,x] = ode45(rhs,t,[0;0]);

    u_cmd = -kp(i)*x(:,1) - kd(i)*x(:,2);
    u = min(max(u_cmd,-Fmax),Fmax);
    acc = (d(t) + u - c*x(:,2) - k*x(:,1))/m;

    a_rms(i) = sqrt(trapz(t,acc.^2)/(t(end)-t(1)));
    u_peak(i) = max(abs(u_cmd));
    saturated(i) = u_peak(i) > Fmax;
end

results = table(k_values/1e3,kp/1e3,kd/1e3,a_rms,u_peak/1e3,saturated, ...
    'VariableNames',{'k_kN_per_m','kp_kN_per_m','kd_kNs_per_m', ...
                     'accel_rms_m_per_s2','commanded_force_peak_kN','saturated'});
disp(results)

figure('Color','w');
tiledlayout(2,1,'TileSpacing','compact');
nexttile
plot(k_values/1e3,a_rms,'o-','LineWidth',1.5)
ylabel('RMS acceleration [m/s^2]'); grid on
nexttile
plot(k_values/1e3,u_peak/1e3,'o-','LineWidth',1.5); hold on
yline(Fmax/1e3,'--r','Actuator limit','LineWidth',1.2)
xlabel('Frozen spring stiffness k [kN/m]')
ylabel('Peak commanded force [kN]'); grid on

function a = acceleration(time,x,k,kp,kd,m,c,Fmax,d)
    u_cmd = -kp*x(1) - kd*x(2);
    u = min(max(u_cmd,-Fmax),Fmax);
    a = (d(time) + u - c*x(2) - k*x(1))/m;
end