% This script demonstrates how to compute a terminal region for the
% platoon benchmark using the linSys algorithm

clear;
yalmip('clear');
rng(1234);
N = 50;
Nsimulation = 10;
tol_verification = 1e-3;

%% Params
% init
Param = param_platoon();

benchmark = 'platoon4';



%% Options
% general options
Opts.N = N;

Opts.timeStep = 0.1;

Opts.taylorOrder = 5;

algorithm = 'linSys';

% Testing 'implicit' methods
Opts.terminalRegionType = 'implicit';

Opts.terminalRegionSet = 'ellipsoid';
Opts.genMethod = 'provided';
Opts.G = speye(dim(Param.X));
t_ell_implicit = tic;
T_ell_implicit = computeTerminalRegion(benchmark, algorithm, Param, Opts);
t_ell_implicit = toc(t_ell_implicit);
disp("Time elapsed for linSys with ellipsoids using the implicit method: " + num2str(t_ell_implicit))

yalmip('clear')
Opts.terminalRegionSet = 'zonotope';
Opts.genMethod = 'spherical';
Opts.nGenerators = 20;
t_zono_implicit = tic;
T_zono_implicit = computeTerminalRegion(benchmark, algorithm, Param, Opts);
t_zono_implicit = toc(t_zono_implicit);
disp("Time elapsed for linSys with zonotopes using the implicit method: " + num2str(t_zono_implicit))

% Comparison with the 'fixed' sub-algorithm
Opts.terminalRegionType = 'fixed';
Opts.xEq = [0; 0; 10; 0; 10; 0; 10; 0];     % equilibrium point
Opts.uEq = [0; 0; 0; 0];                    % control input eq. point

% compute terminal region
t_zono_fixed = tic;
T_zono_fixed = computeTerminalRegion(benchmark, algorithm, Param, Opts);
t_zono_fixed = toc(t_zono_fixed);
disp("Time elapsed for zonoLinSys: " + num2str(t_zono_fixed))


figure; hold on;

pell_implicit = plot(T_ell_implicit.set,[1 2],'b--','DisplayName','linSys - implicit - Ellipsoid');
pzono_implicit = plot(T_zono_implicit.set,[1 2],'r--','DisplayName','linSys - implicit - Zonotope');

pzonoLinSys = plot(T_zono_fixed.set, [1 2], 'g', 'DisplayName','linSys - fixed - Zonotope');

legend();

yalmip('clear')
% Create simulations
t_simulations_ell_implicit = tic;
simulations_ell_implicit = T_ell_implicit.simulateRandom(Nsimulation, 'extreme');
t_simulations_ell_implicit = toc(t_simulations_ell_implicit);
t_simulations_zono_implicit = tic;
simulations_zono_implicit = T_zono_implicit.simulateRandom(Nsimulation, 'extreme');
t_simulations_zono_implicit = toc(t_simulations_zono_implicit);

t_simulations_zono_fixed = tic;
simulations_zono_fixed = T_zono_fixed.simulateRandom(Nsimulation, 'extreme');
t_simulations_zono_fixed = toc(t_simulations_zono_fixed);

disp("Ellipsoid Implicit Verification...")
T_ell_implicit.verifyTrajectory(simulations_ell_implicit,tol_verification);
disp("Zonotope Implicit Verification...")
T_zono_implicit.verifyTrajectory(simulations_zono_implicit,tol_verification);

disp("Zonotope Fixed Verification...")
T_zono_fixed.verifyTrajectory(simulations_zono_fixed,tol_verification);



