%% Satellite - Classical PI (Part A)
%
% Classical control script for Assignment 1 in Fuzzy systems
% 
% author:
%   Christos Choutouridis ΑΕΜ 8997
%   cchoutou@ece.auth.gr
%
% -------------------------------------------------------
% Givens
%                     10
% Plant: Gp(s) = ------------
%                 (s+1)(s+9)
% 
%                              s + c
% PI: Gc(s) = Kp + Ki/s = Kp --------- , with c = Ki/Kp
%                                s

clear; clc; close all;

s = tf('s');

% Givens
% -------------------------------------------------------
Gp = 10/((s+1)*(s+9));          % given plant
spec.Mp_max   = 0.10;           % 10% overshoot
spec.Ts_max   = 2.0;            % 2sec settling time
spec.SteadyStateValue = 1.0;    % step of 1

% Configuration
config.PI_zero = -1;
config.stepresp_time = 5;

% Control
Gc = @(c)     (s + c)/s;            % No gain controller
L0 = @(c)     0.1 * Gc(c) * Gp;     % No gain open loop
Lk  = @(K, c) K * L0(c);            % open-loop with K [requested by the assignment]

% Utilities to extract Kp, Ki from K [variable requested by the assignment]
Kp = @(K)    0.1*K;
Ki = @(K, c) c*Kp(K);

% ---------- Brute force Root-locus exploration for fixed zero ----------
c = -config.PI_zero;                 % zero location
best = struct('cost',Inf);
for K = linspace(0, 100, 400)
    CL = feedback(Lk(K, c), 1, -1);  % closed loop unity feedback
    info = stepinfo(CL);
    if ~isfinite(info.SettlingTime) || isnan(info.Overshoot)
        continue
    end
    % First check hard specs
    if info.Overshoot/100 <= spec.Mp_max && ...
       info.SettlingTime  <= spec.Ts_max
        %
        % Select the best CL based on a more appropriate cost function
        % (ITAE-like): integral of t*|e(t)| approximated by weights
        %
        [y, t] = step(CL, 0:0.001:config.stepresp_time);
        e = spec.SteadyStateValue - y;
        cost = trapz(t, t.*abs(e));
        if cost < best.cost
            best.cost = cost;
            best.K    = K;
            best.info = info;
            best.CL   = CL;
            best.ess  = e(end);
        end
    end
end

fprintf('Best match: Kp=%.3g, Ki=%.3g\n', Kp(best.K), Ki(c, best.K));
fprintf('Poles:\n');
p = pole(best.CL);
for k = 1:numel(p)
    fprintf('  Pole %d: %.5f %+.5fi\n', k, real(p(k)), imag(p(k)));
end
fprintf('Step response:\n');
fprintf('  RiseTime      : %.4g s\n', best.info.RiseTime);
fprintf('  SettlingTime  : %.4g s\n', best.info.SettlingTime);
fprintf('  Overshoot     : %.2f%%\n', best.info.Overshoot);
fprintf('  Peak          : %.4g\n',   best.info.Peak);
fprintf('  PeakTime      : %.4g s\n', best.info.PeakTime);
fprintf('  SS error      : %.4g s\n', best.ess);

f1 = figure( ...
    'Color','w', ...
    'Name',sprintf('Root Locus - c=%.3g', c), ...
    'Position',[100 100 1200 800] ...
);
h = rlocusplot(L0(c));
setoptions(h, 'XLim', [-12 2], 'YLim', [-8 8]);
ax = findobj(h, 'Type','Axes');
set(ax,'NextPlot','add');
grid(ax,'on');

% Overlays
p_cl = pole(best.CL);
hCL = plot(ax, real(p_cl), imag(p_cl), 'ro', 'MarkerSize', 8, ...
           'MarkerFaceColor','r', 'DisplayName', ...
           sprintf('CL poles: K_p=%.3g, K_i=%.3g', Kp(best.K), Ki(best.K, c)));

infoRL = sprintf([ ...
    'K_p = %.3g\n' ...
    'K_i = %.3g\n' ...
    'c  = %.3g\n' ...
    'Poles:\n' ...
    '  %.4f%+.4fi\n' ...
    '  %.4f%+.4fi\n' ...
    '  %.4f%+.4fi'], ...
     Kp(best.K), Ki(c,best.K), c, ...
     real(p_cl(1)), imag(p_cl(1)), ...
     real(p_cl(2)), imag(p_cl(2)), ...
     real(p_cl(3)), imag(p_cl(3)));

p_ol = pole(L0(c));
hOL = plot(ax, real(p_ol), imag(p_ol), 'kx', 'MarkerSize', 9, ...
           'LineWidth', 1.4, 'DisplayName','OL poles');

% z_ol = zero(L0(c));
% hZ  = plot(ax, real(z_ol), imag(z_ol), 's', 'MarkerSize', 8, ...
%            'MarkerFaceColor',[1 0.9 0.2], 'MarkerEdgeColor','k', ...
%            'DisplayName','OL zero');

% Legend - annotations
lg = legend(ax, [hCL hOL], {'CL poles','OL poles'}, ...
            'Location','best','AutoUpdate','off');

annotation(f1, 'textbox', [0.75 0.05 0.4 0.3], ...
    'String', infoRL, 'Interpreter','tex', ...
    'FontName','monospaced', 'FontSize',12, ...
    'FitBoxToText','on', 'BackgroundColor', 'w', 'EdgeColor', [1 1 1]);

% titles
% xlabel(ax,'Real Axis (s^{-1})'); ylabel(ax,'Imaginary Axis (s^{-1})');
% title(ax, sprintf('Root Locus (c=%.3g)', c));
drawnow;

% save figure
exportgraphics(f1, sprintf('Root_Locus_c%.3g.png', c), 'Resolution', 300);
% exportgraphics(f1, sprintf('Root_Locus_c%.3g.pdf', c));


f2 = figure( ...
    'Color','w', ...
    'Name',sprintf('Step responce - c=%.3g, Kp=%.3g, Ki=%.3g', c, Kp(best.K), Ki(best.K, c)), ...
    'Position',[200 200 1200 800] ...
);
step(best.CL);
xlabel('Time (s)');
ylabel('Amplitude');
title(sprintf('Step Response - c=%.3g, Kp=%.3g, Ki=%.3g', c, Kp(best.K), Ki(best.K, c)));
grid on;

infoStep = sprintf(['RiseTime   : %.4g s\n' ...
                    'Settling   : %.4g s\n' ...
                    'Overshoot  : %.2f %%\n' ...
                    'Peak       : %.4g (at %.4g s)\n' ...
                    'SS error   : %.3f'], ...
                   best.info.RiseTime, best.info.SettlingTime, best.info.Overshoot, ...
                   best.info.Peak, best.info.PeakTime, best.ess);

annotation(f2, 'textbox', [0.7 0.05 0.4 0.25], ...
    'String', infoStep, 'Interpreter','tex', ...
    'FontName','monospaced', 'FontSize',12, ...
    'FitBoxToText','on', 'BackgroundColor','w', 'EdgeColor',[1 1 1]);


% save figure
exportgraphics(f2, sprintf('Step_Responce_c%.3g_Kp%.3g_Ki%.3g.png', c, Kp(best.K), Ki(best.K, c)), 'Resolution', 300);
% exportgraphics(f2, sprintf('Step_Responce_c%.3g_Kp%.3g_Ki%.3g.pdf', c, Kp(best.K), Ki(best.K, c)));