%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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;

    % Start algorithm
    omega = Polyhedron.emptySet(params.model.nx);
    n_iter = 0;
    while n_iter < 200
        omega_next = omega + Fi(n_iter);

        fprintf("n_iter=%d: Omega volume=%d, Fi volume=%d, fulldim=%d, bounded=%d\n", ...
            n_iter, omega_next.volume(), Fi(n_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.copy();
    end

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