%% Satellite - Scenario 1 tune FZ-PI
%
% Assignment 1 in Fuzzy systems
% 
% author:
%   Christos Choutouridis ΑΕΜ 8997
%   cchoutou@ece.auth.gr
%

% 
clear; clc; close all;

s = tf('s');

% Givens
% -------------------------------------------------------
Gp = 10/((s+1)*(s+9));          % given plant
spec.Mp_max   = 0.07;           % 7% overshoot
spec.Tr_max   = 0.6;            % 0.6sec rise time
spec.SteadyStateValue = 1.0;    % step of 1

Ts = 0.01;

% Configuration
tEnd = 5;
Ke  = 1;
fis = mkFIS();

best = struct('cost',Inf);
for Kde = 0.2:0.01:0.35
    for Kdu = 0.3:0.01:0.5
        % Convert Gp -> A,B,C,D (z)
        Gz = c2d(Gp, Ts, 'zoh');
        Gd = ss(Gz);
        Ad=Gd.A; Bd=Gd.B; Cd=Gd.C; Dd=Gd.D;
        
        %figure('Name','FLC surface'); gensurf(fis); grid on;
        
        
        t = 0:Ts:tEnd;
        r = ones(size(t));
        x = zeros(size(Ad,1),1);
        y = 0;
        
        % logs
        Y = zeros(size(t)); U = zeros(size(t)); E = zeros(size(t));
        
        clear satFZ_PI;  % Init satFZ_PI
        for k = 1:numel(t)
            % calculate error
            e = r(k) - y;
        
            %evaluate controller
            u = satFZ_PI(fis, e, Ke, Kde, Kdu, Ts);
        
            % plant update (ZOH)
            x = Ad*x + Bd*u;
            y = Cd*x + Dd*u;
                
            % keep logs
            Y(k) = y; U(k) = u; E(k) = e;
        end
        info = stepinfo(Y, t, 1);
        % First check hard specs
        if info.Overshoot/100 <= spec.Mp_max && ...
           info.RiseTime  <= spec.Tr_max
            %
            % Select the best CL based on a more appropriate cost function
            % (ITAE-like): integral of t*|e(t)| approximated by weights
            %
            cost = trapz(t, t.*abs(E));
            if cost < best.cost
                best.cost = cost;
                best.Kdu  = Kdu;
                best.Kde  = Kde;
                best.info = info;
                best.ess  = E(end);
            end
        end
    end
end

fprintf('Best match: Kde=%.3g, Kdu=%.3g\n', best.Kde, best.Kdu);
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);