%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (c) 2023, Amon Lahr, Simon Muntwiler, Antoine Leeman & Fabian Flürenbrock Institute for Dynamic Systems and Control, ETH Zurich.
%
% All rights reserved.
%
% Please see the LICENSE file that has been included as part of this package.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function [H_tube,h_tube,n_iter] = compute_minRPI(K_tube,params)
    % YOUR CODE HERE
    fprintf("\n")

    % Compute closed loop, disturbance set and disturbance reachable sets
    Ak = params.model.A + params.model.B*K_tube;
    W = Polyhedron('A', params.constraints.DisturbanceMatrix, ...
                   'b', params.constraints.DisturbanceRHS);
    Fi = @(i) Ak^i * W;

    fprintf("Stable error dynamics? %d\n", all(eig(Ak) < 1))
    fprintf("Decreasing DRS? %d\n", Fi(3).volume() > Fi(4).volume());

    % Start algorithm
    omega = Polyhedron.emptySet(params.model.nx);
    n_iter = 0;
    while n_iter < 200
        Fn_iter = Fi(n_iter);
        omega_next = omega + Fn_iter;
        fprintf("n_iter=%d: volume=%d, fulldim=%d, bounded=%d\n", ...
            n_iter, Fn_iter.volume(), omega_next.isFullDim(), omega_next.isBounded())

        % why does this happen?
        if ~ omega_next.isFullDim() || ~ omega_next.isBounded()
            fprintf("There is something off ...\n");
            break;
        end

        if eq(omega_next.minHRep(), omega.minHRep())
            fprintf("converged!\n");
            break;
        end
        n_iter = n_iter+1;
        omega = omega_next;
    end

    H_tube = omega.A;
    h_tube = omega.b;
end