% Controller design for a ducted fan VTOL micro-UAV.
%
% Copyright (c) 2024, Naoki Sean Pross, ETH Zürich
% This work is distributed under a permissive license, see LICENSE.txt

%  ------------------------------------------------------------------------
% Clear environment and generate parameters

clear; clc; close all; s = tf('s');

do_plots = true; % runs faster without
do_hinf = true; % midterm
do_musyn = false; % endterm

if do_hinf & do_musyn
  error('Cannot do both H-infinity and mu synthesis.')
end

fprintf('Controller synthesis for ducted fan VTOL micro-UAV\n')
fprintf('Will do:\n')
if do_plots
  fprintf(' - Produce plots\n')
end
if do_hinf
  fprintf(' - H-infinity synthesis\n')
end
if do_musyn
  fprintf(' - Mu synthesis\n')
end

% Synthesized controllers will be stored here
ctrl = struct();

%% ------------------------------------------------------------------------
% Define system parameters

fprintf('Generating system parameters...\n')
params = uav_params();

%% ------------------------------------------------------------------------
% Define performance requirements

if do_hinf
  fprintf('Generating performance requirements...\n')
  perf = uav_performance_hinf(params, do_plots);
end
if do_musyn
  fprintf('Generating performance requirements...\n')
  perf = uav_performance_musyn(params, do_plots);
end

%%  ------------------------------------------------------------------------
% Define stability requirements

% Note: for hinf it is needed to call uav_mode, but hinf will not actually 
% make use of this struct
if do_hinf | do_musyn
  fprintf('Generating stability requirements...\n')
  uncert = uav_uncertainty(params, do_plots);
end

%% ------------------------------------------------------------------------
% Create UAV model

fprintf('Generating system model...\n');
model = uav_model(params, perf, uncert);

%% ------------------------------------------------------------------------
% Perform H-infinity design

if do_hinf
  fprintf('Performing H-infinty controller design...\n')

  idx = model.uncertain.index;
  P = model.uncertain.StateSpace;

  % Get nominal system without uncertainty (for lower LFT)
  P_nom = minreal(P([idx.OutputError; idx.OutputNominal], ...
                    [idx.InputExogenous; idx.InputNominal]), [], false);

  nmeas = model.uncertain.Ny;
  nctrl = model.uncertain.Nu;

  hinfopt = hinfsynOptions('Display', 'on', 'Method', 'RIC', ...
    'AutoScale', 'off', 'RelTol', 1e-3);
  [K_inf, ~, gamma, info] = hinfsyn(P_nom, nmeas, nctrl, hinfopt);
  ctrl.hinf = struct('Name', '$\mathcal{H}_{\infty}$', 'K', K_inf);

  if gamma >= 1
    fprintf('Failed to syntesize controller (closed loop is unstable).\n')
  end

%%  ------------------------------------------------------------------------
% Measure Performance of H-infinity design

  fprintf('Simulating closed loop...\n');

  nsamples = 500;
  do_noise = true;
  simout = uav_sim_step_hinf(params, model, ctrl.hinf, nsamples, do_plots, do_noise);

  fprintf('Writing simulation results...\n');
  cols = [
      simout.StepX(:, simout.index.Position), ...
      simout.StepX(:, simout.index.Velocity), ...
      simout.StepX(:, simout.index.FlapAngles) * 180 / pi, ...
      simout.StepX(:, simout.index.Angles) * 180 / pi];

  writematrix([simout.TimeXY', cols], 'fig/stepsim.dat', 'Delimiter', 'tab')
end

%%  ------------------------------------------------------------------------
% Perform mu-Analysis & DK iteration

if do_musyn
  fprintf('Performing mu-synthesis controller design...\n')

  % Get complete system (without debugging outputs for plots)
  idx = model.uncertain.index;
  P = minreal(model.uncertain.StateSpace(...
        [idx.OutputUncertain; idx.OutputError; idx.OutputNominal], ...
        [idx.InputUncertain; idx.InputExogenous; idx.InputNominal]), ...
        [], false);

  % Options for H-infinity
  nmeas = model.uncertain.Ny;
  nctrl = model.uncertain.Nu;
  hinfopt = hinfsynOptions('Display', 'off', 'Method', 'RIC', ...
    'AutoScale', 'on', 'RelTol', 1e-2);

  % Frequency raster resolution to fit D scales
  nsamples = 500;
  omega = logspace(-5, 5, nsamples);

  % Initial values for D-K iteration
  D_left = tf(eye(model.uncertain.Nz + model.uncertain.Ne + model.uncertain.Ny));
  D_right = tf(eye(model.uncertain.Nv + model.uncertain.Nw + model.uncertain.Nu));

  % degrees for approximations for D-scales, tuned by hand
  fit_degrees = [
    1, 1;  % alpha
    6, 1;  % omega
    1, 1;  % state
    7, 1;  % perf
  ];

  % Number of D-K iterations
  niters = size(fit_degrees, 2);

  % for plotting later
  mu_plot_legend = {};

  % Start DK-iteration
  dkstart = tic;
  for it = 1:niters
    fprintf(' - Running D-K iteration %d / %d...\n', it, niters);
    itstart = tic();

    % Find controller using H-infinity
    [K, ~, gamma, ~] = hinfsyn(D_left * P * D_right, nmeas, nctrl, hinfopt);
    fprintf('   H-infinity synthesis gamma: %g\n', gamma);
    if gamma == inf
      fprintf('   Failed to synethesize H-infinity controller\n');
      break;
    end

    % Calculate frequency response of closed loop
    N = minreal(lft(P, K), [], false); % slient
    N_frd = frd(N, omega);

    % Calculate upper bound D scaling
    [mu_bounds, mu_info] = mussv(N_frd, model.uncertain.BlockStructurePerf, 'sU');
    mu_rp = norm(mu_bounds(1,1), inf, 1e-6);
    fprintf('   Mu value for RP: %g\n', mu_rp)

    if do_plots
      fprintf('   Plotting mu\n');
      figure(100); hold on;
      bodemag(mu_bounds(1,1));
      mu_plot_legend = {mu_plot_legend{:}, sprintf('$\\mu_{%d}$', it)};
      title('\bfseries $\mu_\Delta(\omega)$ for both Stability and Performance', ...
            'interpreter', 'latex');
      legend(mu_plot_legend, 'interpreter', 'latex');
      grid on;
      drawnow;
    end

    % Are we done yet?
    if mu_rp < 1
      fprintf(' - Found robust controller that meets performance.\n');
      break
    end

    % Fit D-scales
    % There are three complex, square, full block uncertainties and
    % a non-square full complex block for performance
    [D_left_samples, D_right_samples] = mussvunwrap(mu_info);

    fprintf('   Fitting D-scales');

    % D scale for alpha uncertainty (first block)
    i = 1;
    D_left_samples_alpha = D_left_samples(i, i);
    D_alpha = fitmagfrd(D_left_samples_alpha, fit_degrees(1, it));
    % D_alpha = fitfrd(genphase(D_left_samples_alpha), fit_degrees(1, it));
    fprintf('.');

    % D scale for omega uncertainty (second block)
    i = model.uncertain.BlockStructure(1, 1) + 1; % after first block
    D_left_samples_omega = frd(D_left_samples(i, i));
    D_omega = fitmagfrd(D_left_samples_omega, fit_degrees(2, it));
    % D_omega = fitfrd(genphase(D_left_samples_omega), fit_degrees(2, it));
    fprintf('.');

    % D scale for state uncertainty (third block)
    i = model.uncertain.BlockStructure(2, 1) + 1; % after second block
    D_left_samples_state = D_left_samples(i, i);
    D_state = fitmagfrd(D_left_samples_state, fit_degrees(3, it));
    % D_state = fitfrd(genphase(D_left_samples_state), fit_degrees(3, it));
    fprintf('.');

    % D scale for performance (non-square)
    i = model.uncertain.BlockStructurePerf(3, 1); % after third block
    D_left_samples_perf = D_left_samples(i, i);
    D_perf = fitmagfrd(D_left_samples_perf, fit_degrees(4, it));
    % D_perf = fitfrd(genphase(D_left_samples_perf), fit_degrees(4, it));
    fprintf('.');

    fprintf('\n');

    % Construct full matrices
    D_right = blkdiag(D_alpha * eye(4), ...
                      D_omega * eye(1), ...
                      D_state * eye(12), ...
                      D_perf * eye(10), ...
                      eye(5));

    D_left = blkdiag(D_alpha * eye(4), ...
                     D_omega * eye(1), ...
                     D_state * eye(12), ...
                     D_perf * eye(14), ...
                     eye(12));

    % Compute peak of singular values for to estimate how good is the
    % approximation of the D-scales
    sv_left_samples = sigma(D_left_samples);
    max_sv_left_samples = max(max(sv_left_samples));

    sv_left = sigma(D_left, omega);
    max_sv_left = max(max(sv_left));

    fprintf('   Max SVD of D: %g, Dhat: %g\n', max_sv_left_samples, max_sv_left);
    fprintf('   D scales fit abs. error: %g\n', abs(max_sv_left_samples - max_sv_left));

    % Plot fitted D-scales
    if do_plots
      fprintf('   Plotting D-scales');
      f = figure(101); clf(f); hold on;

      bodemag(D_left_samples_alpha, omega, 'r-');
      bodemag(D_alpha, omega, 'b');
      fprintf('.');

      bodemag(D_left_samples_omega, omega, 'r--');
      bodemag(D_omega, omega, 'b--');
      fprintf('.');

      bodemag(D_left_samples_state, omega, 'c-');
      bodemag(D_state, omega, 'm-');
      fprintf('.');

      bodemag(D_left_samples_perf, omega, 'c--');
      bodemag(D_perf, omega, 'm--');
      fprintf('.');

      fprintf('\n');
      title(sprintf('\\bfseries $D(\\omega)$ Scales Approximations at Iteration %d', it), ...
            'interpreter', 'latex')
      legend(...
        '$D_{\alpha}$', '$\hat{D}_{\alpha}$', ...
        '$D_{\omega}$', '$\hat{D}_{\omega}$', ...
        '$D_{\mathbf{x}}$', '$\hat{D}_{\mathbf{x}}$', ...
        '$D_{\Delta}$', '$\hat{D}_{\Delta}$', ...
        'interpreter', 'latex' ...
      );
      grid on;
      drawnow;
    end

    itend = toc(itstart);
    fprintf('   Iteration took %.1f seconds\n', itend);
  end
  dkend = toc(dkstart);
  fprintf(' - D-K iteration took %.1f seconds\n', dkend);

  if mu_rp > 1
    fprintf(' - Failed to synthesize robust controller that meets the desired performance.\n');
  else
    ctrl.musyn = struct('K', K, 'mu', mu_rp);
  end
end