% Control System Design for F-16 Aircraft
clear; clc; close all;

% Task 1: Basic understanding of the system
% (a) Unit step response
G = tf([-0.0717 -1.684 -0.0853 -0.0622], [1 1.0604 -1.1154 -0.066 -0.0512]);
figure;
step(G);
title('Unit Step Response of G(s)');

% (b) Poles and zeros
pzmap(G);
title('Pole-Zero Map of G(s)');

%% Task 2: Making the system compatible for Bode plot-based controller design
% (a) Finding Kf using the Nyquist plot
figure;
nyquist(G);
title('Nyquist Plot of G(s)');
Kf = -1.2; % Choose Kf such that G_tilde(s) is stable

% (b) Check stability of G_tilde(s)
G_tilde = G / (1 + Kf * G);
figure;
step(G_tilde);
title('Unit Step Response of G_tilde(s)');
pzmap(G_tilde);
title('Pole-Zero Map of G_tilde(s)');

% (c) G_tilde(s) does not meet the design criteria

% Task 3: Meeting the steady-state error criteria
% (a) Design C1(s) as a Type 1 system
C1 = tf([10 10], [1 0]); % C1(s) = 10(s+1)/s
disp('C1(s):');
pretty(C1)

% Task 4: Meeting the settling time and maximum overshoot criteria
% (a) Bode plot of H_tilde(s) = -C1(s)G_tilde(s)
H_tilde = -C1 * G_tilde;
figure;
bode(H_tilde);
grid on;
[Gm, Pm, Wpm, Wgm] = margin(H_tilde);
fprintf('Gain Margin = %.2f dB\n', Gm);
fprintf('Phase Margin = %.2f deg\n', Pm);

% (b) Design C2(s) as a phase-lead compensator
% Step 1: Choose the structure of C2(s)
% Using the Bode plot of H_tilde(s), a phase-lead compensator structure
% is chosen: C2(s) = K * (s + z) / (s + p)
% where z > p to provide phase lead

% Step 2: Choose the zero (z) and pole (p) locations
% For a phase-lead compensator, a typical choice is:
z = 5; % Zero location
p = 0.2; % Pole location

% Step 3: Adjust the gain K to meet the settling time requirement
% Start with a low value of K and increase it until the settling time
% requirement is met while maintaining stability (positive phase margin)
K = 20; % Initial gain value

C2 = tf([K * z K * p], [1 z 0]); % C2(s) = K * (s + z) / (s + p)
H_tilde_CL = H_tilde * C2;

% Iterate and adjust K until the settling time requirement is met
settling_time_met = false;
while ~settling_time_met
    figure;
    bode(H_tilde_CL);
    grid on;
    [Gm_CL, Pm_CL, Wpm_CL, Wgm_CL] = margin(H_tilde_CL);
    fprintf('Closed-Loop Gain Margin = %.2f dB\n', Gm_CL);
    fprintf('Closed-Loop Phase Margin = %.2f deg\n', Pm_CL);

    if Pm_CL > 0 && Gm_CL > 0
        % Check if the settling time requirement is met
        S = stepinfo(H_tilde_CL, 'SettlingTimeThreshold', 0.02);
        if S.SettlingTime <= 0.05
            settling_time_met = true;
            fprintf('Settling time requirement met.\n');
        else
            K = K * 1.2; % Increase the gain
            C2 = tf([K * z K * p], [1 z 0]);
            H_tilde_CL = H_tilde * C2;
        end
    else
        % System is unstable, reduce the gain
        K = K * 0.8;
        C2 = tf([K * z K * p], [1 z 0]);
        H_tilde_CL = H_tilde * C2;
    end
end

% Final design of C2(s)
disp('C2(s):');
pretty(C2)

% Task 5: Closed-loop simulation
% (a) State-space model of the controller
C = -C1 * C2;
[num_C, den_C] = tfdata(C, 'v');
A_C = [0 0 1; 0 0 0; -den_C(1) -den_C(2) -den_C(3)];
B_C = [0 0; 0 0; num_C(1) num_C(2)];
C_C = [1 0 0; 0 1 0];
D_C = [0 0; 0 0];
sys_C = ss(A_C, B_C, C_C, D_C);

% (b) State-space model of G(s)
[num_G, den_G] = tfdata(G, 'v');
[A_G, B_G, C_G, D_G] = tf2ss(num_G, den_G);

% (c) State-space model of the closed-loop system
A_CL = [A_G zeros(size(A_G,1), size(B_C,2)); B_C*C_G A_C];
B_CL = [B_G; zeros(size(B_C,1), size(B_G,2))];
C_CL = [C_G zeros(size(C_G,1), size(C_C,2))];
D_CL = [D_G zeros(size(D_G,1), size(D_C,2))];
sys_CL = ss(A_CL, B_CL, C_CL, D_CL);

% (d) Simulator function
function simulator(sys_CL, A, omega, alpha0, alphadot0)
    % Reference input
    t = 0:0.01:20;
    r = [zeros(1,501) ones(1,1500)];

    % Disturbance input
    epsilon = A * sin(omega * t);

    % Initial conditions
    x0 = [alpha0; alphadot0; zeros(size(A_CL,1)-2,1)];

    % Simulate closed-loop response
    [y, ~, ~] = lsim(sys_CL, [r; epsilon], t, x0);

    % Plot the response
    figure;
    plot(t, y);
    xlabel('Time (s)');
    ylabel('Attack Angle \alpha(t)');
    title('Closed-Loop Response');
    grid on;
end

%% Run the simulator
A = 0.1; % Amplitude of disturbance
omega = 1; % Frequency of disturbance
alpha0 = 0.1; % Initial attack angle
alphadot0 = 0.05; % Initial rate of change of attack angle
simulator(sys_CL, A, omega, alpha0, alphadot0);