function res = test_computTermRegNonlinSysLinApproach_trustRegionExploitation
%TEST_COMPUTTERMREGNONLINSYSLINAPPROACH_TRUSTREGIONEXPLOITATION - unit test
%function of computeTrustRegionExploitation.
%
% Syntax:
%       res = test_computTermRegNonlinSysLinApproach_trustRegionExploitation
%
% Input Arguments:
%       -
%
% Output Arguments:
%       -res:   indicates whether the test has been passed or not
%               (boolean)
%
% See Also:
%       @computeTermRegNonlinSysLinApproach/computeTrustRegionExploitation
% 
% References:
%       -
%
%------------------------------------------------------------------
% This file is part of <a href="matlab:docsearch aroc">AROC</a>, a Toolbox for Automatic Reachset-
% Optimal Controller Synthesis developed at the Chair of Robotics, 
% Artificial Intelligence and Embedded Systems, 
% Technische Universitaet Muenchen. 
%
% For updates and further information please visit <a href="http://aroc.in.tum.de">aroc.in.tum.de</a>
%
% More Toolbox Info by searching <a href="matlab:docsearch aroc">AROC</a> in the Matlab Documentation
%
%------------------------------------------------------------------
% Authors:      Lukas Schäfer
% Website:      <a href="http://aroc.in.tum.de">aroc.in.tum.de</a>
% Work Adress:  Technische Universitaet Muenchen
% Copyright (c) 2024 Chair of Robotics, Artificial Intelligence and
%               Embedded Systems, TU Muenchen
%------------------------------------------------------------------ 

% instantiate solver object
nx = 3;
nu = 2;
nw = 1;
% ... algorithm settings
opts.termRegOrder = 3;
opts.xEq = rand(nx,1);
opts.uEq = rand(nu,1);
opts.verbose = 0;
% ... parameters
param.X = zonotope.generateRandom('Center',opts.xEq,'Dimension',nx);
param.U = zonotope.generateRandom('Center',opts.uEq,'Dimension',nu);
param.W = zonotope.generateRandom('Center',zeros(nw,1),'Dimension',nw);

solver = computeTermRegNonlinSysLinApproach(param,opts);

nGen = opts.termRegOrder*nx;

% -------------- random testing ------------------------------------------
nRandCases = 10;
resRand = false(nRandCases,1);

for ii = 1:nRandCases
    % genrate random terminal region, trust region and generator scaling
    % factors
    termReg = zonotope.generateRandom("Dimension",nx,"NrGenerators",nGen);
    tr = interval.generateRandom("Dimension",nx,'Center',zeros(nx,1));
    s = rand(nGen,1) + 0.6;

    % set trust region
    solver.tr_set = zonotope(tr);

    % compute expected value
    trExploitation_exp = 0;
    IH_delta_termReg_scale = interval(zonotope(zeros(nx,1),termReg.G*diag(s)));
    IH_delta_termReg = interval(zonotope(zeros(nx,1),termReg.G));

    for jj = 1:nx
        trExploitation_exp = max(trExploitation_exp, ...
            (IH_delta_termReg_scale.sup(jj)-IH_delta_termReg.sup(jj)) ...
            /(IH_delta_termReg.sup(jj)+tr.sup(jj) - IH_delta_termReg.sup(jj)));
    end
    
    % evaluate function under test and compare results
    resRand(ii) = withinTol(trExploitation_exp, ...
        solver.computeTrustRegionExploitation(s,termReg));
end

% -------------- edge cases ----------------------------------------------
resEdge = false(3,1);

termReg = zonotope.generateRandom("Dimension",nx,"NrGenerators",nGen);

% (1): candidate terminal region contained in box enclosure of initial 
% guess for the terminal region, i.e., trust region exploitation = 0
s = rand(nGen,1);
scale = scaleZonoIntoZono(zonotope(termReg.c,termReg.G*diag(s)), ...
    zonotope(interval(termReg)));
resEdge(1) = withinTol(0, ...
    solver.computeTrustRegionExploitation(scale*s,termReg),1e-6);

%(2): candidate terminal region has non-empty intersection with boundary of 
% initial guess for the terminal region + trust region, i.e., trust region 
% exploitation = 1
scale = scaleZonoIntoZono(zonotope(termReg.c,termReg.G), ...
    zonotope(interval(termReg+solver.tr_set)));
resEdge(2) = withinTol(1, ...
    solver.computeTrustRegionExploitation(scale*ones(nGen,1),termReg),1e-6);

%(3): candidate terminal region is identical to the initial guess for the 
% terminal region, i.e., trust region exploitation = 0
resEdge(3) = withinTol(0, ...
    solver.computeTrustRegionExploitation(ones(nGen,1),termReg));

%(4): candidate terminal region is a strict subset of (box enclosure) of
%the initial guess for the terminal region, i.e., trust region 
% exploitation = 0
resEdge(4) = withinTol(0, ...
    solver.computeTrustRegionExploitation(0.8*ones(nGen,1),termReg));

res = all(resRand) && all(resEdge);

end
