% Exported code cells from Final_Submission.mlx.
% Team: Alazar Wake, Mahteb Negussie, Nahom Megerssa, Yetemegeta Sebisbe.
% Original calculations retained; MATLAB rerun not performed for this portfolio.

clear;close all;clc;

%% System Parameters
fprintf('── System Paramters and Setup ─────────────────────\n')

J  = 1;      % Wind turbine inertia (kg.m^2)
B  = 1.5;    % Damper coefficient (N.m.rad/sec)
Tp = 0.2;    % Actuator time constant
A  = 10;     % Amplifier (nominal controller) gain
fprintf('(a) Unit Step Response\n')
Gp = tf(1, [Tp, 1]);                        % Pitch actuator: 1/(0.2s+1)
Gd = tf(1, [J,  B]);                        % Drive train:    1/(s+1.5)
LoopShaping = tf(A,  [1,  0]);              % Integrator+gain: 10/s
G_inner = feedback(Gd, 1);                    % Inner feedback: Gd/(1+Gd)
G = series(LoopShaping, series(G_inner, Gp))  % Open-loop G(s)
H = feedback(G, 1)                    % Closed-loop H(s)

figure(1);step(H)
title("Step Response of Original System")

syms s t
%% Transfer Functions
Gp_s = 1 / (Tp*s +1); 
Gd_s = 1 / (J*s + B); 
k_s = A / s;          
Gdf_s = Gd_s/(Gd_s +1); 

G_s = simplify(k_s * Gdf_s * Gp_s); % Open-loop G(s) (symbolic)
H_s = simplify(G_s/(G_s+1));  % Closed-loop H(s) (symbolic)

fprintf('── (b–e) Time Domain Analysis ─────────────────────\n')

% Question (b): Time-domain output for unit step input
fprintf('(B) Time Domain Equation\n')

Y_s = H_s * (1/s);          %<-- multiply by Laplace of unit step
Y_t = vpa(ilaplace(Y_s),4)  % Y(t) = L⁻¹{Y(s)}

t_vals = linspace(0, 10, 1000);

% Manual equation from partial fractions (handwritten derivation)
y_manual = 1 - 0.15*exp(-6.75*t_vals) - exp(-0.375*t_vals) .* (0.85*cos(2.7*t_vals) ...
    + 0.5*sin(2.7*t_vals));

% (c): Evaluate Y(t) at t = 0.5 s
fprintf('(C) Output Pitch angle at t = 0.5 sec\n')
Y_t_05 = double(real(subs(Y_t, t, 0.5))) % Output pitch angle at t = 0.5 s 

% Numeric evaluation for plotting
y_vals = double(real(subs(Y_t, t, t_vals)));   % evaluate numerically


% Overlay symbolic Y(t) , Y_manual and tf step() to confirm they match
figure(2);
plot(t_vals, y_vals,   'g--',  'LineWidth', 2); hold on;
plot(t_vals, y_manual, 'r:',   'LineWidth', 2);
step(H, 10);
hold off;
title('Y(t): Symbolic vs Manual vs step()');
xlabel('Time (s)'); ylabel('Amplitude');
legend('Symbolic Y(t)', 'Manual (partial fractions)', 'step() [tf]');
grid on;


%% ── (d–e) Transient Performance Metrics ─────────────────────────────────
fprintf('── (d–e) Transient Performance Metrics ─────────────────────\n')
 
S = stepinfo(H);
fprintf('  Rise Time      Tr = %.4f s\n',  S.RiseTime)
fprintf('  Settling Time  Ts = %.4f s\n',  S.SettlingTime)
fprintf('  Overshoot      OS = %.4f %%\n', S.Overshoot)
fprintf('  Peak Time      Tp = %.4f s\n\n',S.PeakTime)


%% ── (f–g) Steady-State Output and Error ─────────────────────────────────
fprintf('── (f–g) Steady-State Output & Error ───────────────────────\n')
 
yss = dcgain(H);
ess = 1 - yss;
fprintf('  Steady-State Output   yss = %.4f\n',  yss)
fprintf('  Steady-State Error    ess = %.4f\n\n', ess)
fprintf('  Type 1 system, Theoretical ess = 0 for step input. ✓\n\n')

fprintf('\n========================================\n')
fprintf('   (h-k) Frequency Domain & Root Locus   \n')
fprintf('========================================\n\n')
figure(3);
margin(G)
title('(h–i) Open-Loop Bode Plot with Stability Margins');

[Gm, Pm, Wcg, Wcp] = margin(G);
fprintf('  Gain  Margin  GM  = %.4f dB   (linear: %.4f)\n', 20*log10(Gm), Gm)
fprintf('  Phase Margin  PM  = %.4f deg\n',  Pm)
fprintf('  Phase Crossover   = %.4f rad/s  (where |∠G| = -180°)\n', Wcg)
fprintf('  Gain  Crossover   = %.4f rad/s  (where |G|  =   0 dB)\n\n', Wcp)


fprintf('── (j) Stability Assessment ────────────────────────────────\n')
figure(4);
cl_poles = pole(H); % Calculate closed-loop poles
pzmap(H); title('(j) Pole-Zero Map - Closed-Loop'); grid on;

fprintf('(j) System Stability\n')
fprintf('  Closed-Loop Poles:\n'); disp(cl_poles)

fprintf('  -- Method 1 (Poles): ')
fprintf('All poles in LHP -> System is STABLE\n')
fprintf('  -- Method 2 (Margins): ')
fprintf('PM = %.2f deg (>0) and GM = %.2f dB (>0) -> STABLE\n', Pm, 20*log10(Gm))

% Question (k): Root locus
fprintf('── (k) Root Locus ──────────────────────────────────────────\n\n')
figure(5);
rlocus(G);
title('(k)  Root Locus — Open-Loop G(s)', 'FontSize', 13);
grid on;


[mag, phase, W_out] = bode(G);

mag = squeeze(mag); % Convert 3-D array → column vector (linear)
mag_dB = 20*log10(mag); % Convert to dB

phase = squeeze(phase); % Phase in degrees


%% (L) Stability Comment from Root Locus
fprintf('\n=== (l) Root Locus Stability Comment ===\n')
fprintf('  Open-loop poles: s=0, s=-2.5, s=-5 -> all in LHP (stable at K=0).')
fprintf('  Two locus branches break away from the real axis, enter the complex plane,\n')
fprintf('  and eventually cross the imaginary axis at K = Kcr (= %.4f).\n', Gm)
fprintf('  System is STABLE for:         0 < K < Kcr = %.4f\n', Gm)
fprintf('  System is MARGINALLY STABLE at K = Kcr = %.4f\n', Gm)
fprintf('  System is UNSTABLE for:       K > Kcr = %.4f\n\n', Gm)

%% (M) K for Critical Stability
fprintf('── (m) Critical Stability  K = Kcr = %.4f ───────────────────────────\n', Gm)

K_cr    = Gm;
H_cr  = feedback(K_cr * G, 1);

figure(6);
step(H_cr, 30);
title('Step Response at K = Kcr = 1.8750')
xlabel('Time (s)'); ylabel('Amplitude');
grid on;


%% ── (N) Breakaway Point — K for Critically Damped Response (ζ = 1) ──────────
fprintf('── (n) Breakaway Point — Critically Damped (ζ = 1) ─────────\n')

% dK/ds = 0 gives: 3s² + 15s + 12.5 = 0
bp_r = roots([3, 15, 12.5]);

% Pick the root between s = 0 and s = -2.5 (on the real-axis locus)
s_bp = bp_r(real(bp_r) > -2.5 & real(bp_r) < 0);

% Gain at breakaway: K = |−s(s+2.5)(s+5)/50|
K_bp = abs(s_bp * (s_bp + 2.5) * (s_bp + 5) / 50);

H_bp = feedback(K_bp * G, 1);

fprintf('Breakaway point s = %.4f,  K = %.4f  ->  zeta = 1 (critically damped)\n', real(s_bp), K_bp)

figure(7);
step(H, H_bp, 30);
legend('Original', 'Breakaway (ζ = 1)');
title('(n)  Step Response: Original vs Breakaway Point')
xlabel('Time (s)'); ylabel('Amplitude');
grid on;

%% ── (O) Gain K for 5% Overshoot via Dominant Pole Approximation ─────────────
fprintf('── (o) Gain K for 5%% Overshoot ────────────────────────────\n')

%  Damping ratio for 5% OS
zeta_5 = -log(0.05) / sqrt(pi^2 + log(0.05)^2);   % ζ ≈ 0.69

% Natural frequency
wn_5 = 2.5 / (2 * zeta_5);     % ωn = σ/ζ  [rad/s]

K_5os = wn_5^2 / 50;            % K = ωn²/50

% Step 4: Build and verify closed-loop
H_5os = feedback(K_5os * G, 1);
S_5os = stepinfo(H_5os);

fprintf('  Target ζ  = %.4f  (from 5%% OS formula)\n',   zeta_5)
fprintf('  ωn        = %.4f rad/s\n',                     wn_5)
fprintf('  Gain K    = %.4f\n',                           K_5os)
fprintf('  Achieved OS = %.2f%%  (target 5%% — gap due to 3rd pole)\n\n', S_5os.Overshoot)

figure(8);
step(H, H_5os, 30);
legend('Original', '5% OS (K adjusted)');
title('(O) Step Response: Original vs K for 5% OS');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;

%% Question (p): Gain Compensator Design
fprintf('──(p) Gain Compensator ────────────────────────────\n')

PM_p = Pm + 10;             % New target PM = current PM + 10°

% Find corresponding phase angle on Bode plot
Y_p = -180 + PM_p;          % Target phase = -180° + PM_target

[~, idx_p] = min(abs(phase - Y_p));   % Index where phase ≈ Y_p

% Read magnitude at that frequency (dB)
mag_dB_p = 20 * log10(mag(idx_p));    % |G(jω)| in dB at target frequency

%Compute gain K to bring magnitude to 0 dB at that point
K_gc = 10^(-mag_dB_p / 20);           % K = 10^(−|G|_dB / 20)

H_gc = feedback(K_gc * G, 1);         % Closed-loop with gain compensator
[Gm_gc,Pm_gc,  Wcg_g, Wcp_g] = margin(K_gc * G); % Store compensated margins

fprintf('  Target phase margin  = %.4f°\n',   PM_p)
fprintf('  Target phase  = %.4f°\n',   Y_p)
fprintf('  G_dB at target frequency      = %.4f dB\n', mag_dB_p)
fprintf('  G_dB target      = %.4f dB\n', K_gc)
fprintf('  Achieved PM = %.4f°  (target 80°)\n',   Pm_gc)
fprintf('  Achieved GM = %.4f dB\n\n',20*log10(Gm_gc))

figure(9);
margin(G*K_gc);
title("Bode Plot for Gain Compensated System")


% (Q) Plot 1: Original vs Gain Compensator
figure(10);
step(H, feedback(K_gc*G, 1), 30);
legend('Original', 'Gain Compensator');
title('Step Response: Original vs Gain Compensator');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;

fprintf('──(r) Phase-Lead Compensator ────────────────────────────\n')

PM_W = 80; % Desired phase margin  [degrees]

phi = PM_W - Pm ;  % φ = PM_desired − PM_current  [degrees]

alpha = (1+sind(phi))/(1-sind(phi)); % Calculate alpha, alpha > 1 for lead

G_dB = -10*log10(alpha); % Target magnitude at W_m (compensator adds −G_dB correction)

[~,index_w] = min(abs(mag_dB - G_dB)); % Find frequency W_m where |G(jω)|_dB ≈ G_dB

W_m = W_out(index_w); % New gain crossover frequency  [rad/s]

tau = 1/(W_m * sqrt(alpha)); % τ = 1 / (W_m · √α)

G_lead = tf([alpha*tau 1],[tau 1]); % C_lead(s) = (ατs+1)/(τs+1)

Gc_lead = series(G,G_lead); % Open-loop with lead: G·C_lead
Hc_lead = feedback(Gc_lead, 1);   % Close the lead-compensated loop
[Gm_l,Pm_l,  Wcg_l, Wcp_l] = margin(Gc_lead); % Store compensated margins


fprintf('  φ  = %.4f°\n',   phi)
fprintf('  α                = %.4f\n',     alpha)
fprintf('  G_dB target      = %.4f dB\n', G_dB)
fprintf('  W_m              = %.4f rad/s\n', W_m)
fprintf('  τ                = %.4f s\n\n', tau)
fprintf('  C_lead(s) = (%.4f·s + 1) / (%.4f·s + 1)\n\n', alpha*tau, tau)
fprintf('  Achieved PM = %.4f°  (target 80°)\n',   Pm_l)
fprintf('  Achieved GM = %.4f dB\n\n',20*log10(Gm_l))

figure(11);
margin(G_lead)
title('(r)  Phase-Lead Compensator Open-Loop')
figure(12);
margin(Gc_lead)
title('(r)  Phase-Lead Compensated Open-Loop')


% Plot 2: Original vs Phase-Lead 
figure(13);
step(H, Hc_lead, 30);
legend('Original', 'Phase-Lead');
title('Step Response: Original vs Phase-Lead');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;

fprintf('──(r) Phase-Lag Compensator ────────────────────────────\n')

pk = -180 + PM_W; % Target phase value  [degrees]

[~, index_lag] = min(abs(phase - pk));
 
W_s = W_out(index_lag); % Desired new gain crossover frequency  [rad/s]

tau_lag = 10/W_s; % τ_lag = 10 / W_s  (zero at 1/τ_lag ≪ W_s)

G_lag_dB = 20 * log10(mag(index_lag)); % |G(jW_s)|  in dB

Beta = 10^(G_lag_dB/20); % β = 10^(|G|_dB / 20);  pole at 1/(β·τ_lag)

G_lag = tf([tau_lag 1],[Beta * tau_lag 1]);   % C_lag(s) = (τs+1)/(βτs+1)

Gc_lag = series(G,G_lag); % Open-loop with lag: G·C_lag
Hc_lag  = feedback(Gc_lag,  1);   % Close the lag-compensated loop
[Gm_lg,Pm_lg,  Wcg_lg, Wcp_lg] = margin(Gc_lag); % Store compensated margins

fprintf('  Target phase  pk = %.2f°\n',   pk)
fprintf('  W_s              = %.4f rad/s\n', W_s)
fprintf('  τ_lag            = %.4f s\n',   tau_lag)
fprintf('  |G| at W_s       = %.4f dB\n', G_lag_dB)
fprintf('  β (attenuation)  = %.4f\n\n',  Beta)
fprintf('  C_lag(s) = (%.4f·s + 1) / (%.4f·s + 1)\n\n', tau_lag, Beta*tau_lag)
fprintf('  Achieved PM = %.4f°  (target 80°)\n',  Pm_lg)
fprintf('  Achieved GM = %.4f dB\n\n',            20*log10(Gm_lg))


figure(14);
margin(G_lag)
title('(r)  Phase-Lag Compensator Open-Loop')
figure(15);
margin(Gc_lag)
title('(r)  Phase-Lag Compensated Open-Loop')

% ── Plot 3: Original vs Phase-Lag ────────────────────────────────────────────
figure(16);
step(H, Hc_lag, 100);
legend('Original', 'Phase-Lag');
title('Step Response: Original vs Phase-Lag');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;

%% Steady-State Values 
fprintf('──(s) Performance Comparison ────────────────────────────\n')
ss_orig = dcgain(H);              % Final output of original system
ss_lead = dcgain(Hc_lead);        % Final output of lead system
ss_lag  = dcgain(Hc_lag);         % Final output of lag system

%% Performance Table
S_lead = stepinfo(Hc_lead);
S_lag  = stepinfo(Hc_lag);

specs = {'Rise Time (s)'; 'Settling Time (s)'; 'Overshoot (%)'; ...
         'SS Error'; 'Phase Margin (deg)'; 'Gain Margin (dB)'};

original = [S.RiseTime;      S.SettlingTime;      S.Overshoot;      1-ss_orig; Pm;    20*log10(Gm)];
lead     = [S_lead.RiseTime; S_lead.SettlingTime; S_lead.Overshoot; 1-ss_lead; Pm_l;  20*log10(Gm_l)];
lag      = [S_lag.RiseTime;  S_lag.SettlingTime;  S_lag.Overshoot;  1-ss_lag;  Pm_lg; 20*log10(Gm_lg)];

T = table(original, lead, lag, ...
    'VariableNames', {'Original','Lead','Lag'}, ...
    'RowNames', specs);
disp(T)

%% ── Step Response Overlay ────────────────────────────────────────────────────
figure;
step(H, Hc_lead, Hc_lag, 70);
legend('Original', 'Phase Lead', 'Phase Lag');
title('Step Response Comparison');
grid on;

fprintf('──(t) PID Controller ────────────────────────────\n')

%% ── Step 1: Extract critical values from earlier margin() call ───────────────
K_cr = Gm;                    % Critical gain  (linear, from gain margin)
W_cr = Wcg;                   % Phase crossover frequency  [rad/s]
P_cr = 2*pi / W_cr;           % Critical period  [s]

%% ── Step 2: Ziegler-Nichols tuning formulas ──────────────────────────────────
Kp = 0.6  * K_cr;             % Proportional gain
Ti = 0.5  * P_cr;             % Integral time constant
Td = 0.125* P_cr;             % Derivative time constant
Ki = Kp / Ti;                 % Ki = Kp/Ti  (for pid() object)
Kd = Kp * Td;                 % Kd = Kp*Td  (for pid() object)

fprintf('ZN Tuning Parameters:\n')
fprintf('  Kcr = %.4f,  Pcr = %.4f \n', K_cr, P_cr)
fprintf('  Kp  = %.4f\n', Kp)
fprintf('  Ti  = %.4f s,  Ki = %.4f\n', Ti, Ki)
fprintf('  Td  = %.4f s,  Kd = %.4f\n', Td, Kd)

%% Build PID controller tf() version
Gc_pid_tf = tf(Kp * [Td, Ti, 1], [Ti, 0]);

% pid() version 
C_PID = pid(Kp, Ki, Kd)

%% ── Step 4: Form open-loop and closed-loop ───────────────────────────────────
G_pid_ol = C_PID * G;              % Open-loop with PID
G_pid_cl = feedback(G_pid_ol, 1); % Closed-loop

%% ── Step 5: Verify margins and performance ───────────────────────────────────
[Gm_pid, Pm_pid] = margin(G_pid_ol);
fprintf('\nPID Compensated Margins:\n')
fprintf('  Phase Margin = %.4f deg\n', Pm_pid)
fprintf('  Gain  Margin = %.4f dB\n',  20*log10(Gm_pid))

figure(16);
margin(C_PID);
title('PID Compensator C(s) — Bode Plot');

figure(17);
margin(G_pid_ol);
title('PID Compensated Open-Loop — Bode Plot');

figure(18);
step(H, G_pid_cl, 30);
legend('Original', 'PID (Ziegler-Nichols)');
title('Step Response: Original vs PID');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;


%% ── Performance Comparison — All Five Systems ────────────────────────────────

% Closed-loop systems
G_pid_cl = feedback(C_PID * G, 1);

% Steady-state values
ss_orig = dcgain(H);
ss_lead = dcgain(Hc_lead);
ss_lag  = dcgain(Hc_lag);
ss_gc   = dcgain(H_gc);

[y_pid, ~] = step(G_pid_cl, 100);
ss_pid = y_pid(end);

% Step info
S_gc   = stepinfo(H_gc);
S_pid  = stepinfo(G_pid_cl);

% Margins
[Gm_pid, Pm_pid] = margin(C_PID * G);

% Build table
specs = {'Rise Time (s)'; 'Settling Time (s)'; 'Overshoot (%)'; ...
         'SS Error'; 'Phase Margin (deg)'; 'Gain Margin (dB)'};

col_orig = [S.RiseTime;      S.SettlingTime;      S.Overshoot;      1-ss_orig; Pm;     20*log10(Gm)];
col_gc   = [S_gc.RiseTime;   S_gc.SettlingTime;   S_gc.Overshoot;   1-ss_gc;   Pm_gc;  20*log10(Gm_gc)];
col_lead = [S_lead.RiseTime; S_lead.SettlingTime; S_lead.Overshoot; 1-ss_lead; Pm_l;   20*log10(Gm_l)];
col_lag  = [S_lag.RiseTime;  S_lag.SettlingTime;  S_lag.Overshoot;  1-ss_lag;  Pm_lg;  20*log10(Gm_lg)];
col_pid  = [S_pid.RiseTime;  S_pid.SettlingTime;  S_pid.Overshoot;  1-ss_pid;  Pm_pid; 20*log10(Gm_pid)];

T_all = table(col_orig, col_gc, col_lead, col_lag, col_pid, ...
    'VariableNames', {'Original','GainComp','Lead','Lag','PID'}, ...
    'RowNames', specs);

fprintf('\n========================================\n')
fprintf('       Full Performance Comparison       \n')
fprintf('========================================\n\n')
disp(T_all)


%% ── Comparison Plots: Original vs Each Compensator ──────────────────────────

fprintf('\n========================================\n')
fprintf('         Step Response Comparisons       \n')
fprintf('========================================\n\n')

% All five systems overlaid
figure(19);
step(H, H_gc, Hc_lead, Hc_lag, G_pid_cl, 70);
legend('Original', 'Gain Compensator', 'Phase-Lead', 'Phase-Lag', 'PID (ZN)');
title('Step Response: All Systems Comparison');
xlabel('Time (s)'); ylabel('Amplitude');
grid on;