%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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, h] = lqr_maxPI(Q, R, params)
%     % Define the linear system
%     A = params.model.A;
%     B = params.model.B;
%     
%     % Define the state and input constraints
%     s_max=params.constraints.MaxAbsPositionXZ;
%     y_max=params.constraints.MaxAbsPositionY;
%     u_max = params.constraints.MaxAbsThrust;
%     
%     % Define the cost function
%     [K, ~, ~] = dlqr(A, B, Q, R);
%     P = dare(A, B, Q, R)
%     F = -inv((B'*P*B+R))*B'*P*A
%     A
%     B
%     Ax=[1,0;
%         0,1;
%         -1,0;
%         0,-1];
%     Ax=[1,0;
%         -1,0;
%         0,1;
%         0,-1];
%         Au=Ax;
%     bx=[s_max;s_max;s_max;s_max];
%     bu=[u_max;u_max;u_max;u_max];
%     H=[Ax;Au*F]/2
%     h=[bx;bu]
%     end

function [H, h] = lqr_maxPI(Q, R, params)
    % Define the linear system
    A = params.model.A;
    B = params.model.B;
    nx=params.model.nx;
    nu=params.model.nu;
    
    % Define the state and input constraints
    s_max= params.constraints.MaxAbsPositionXZ;
    y_max= params.constraints.MaxAbsPositionY;
    u_max = params.constraints.MaxAbsThrust;
    
    % Define the cost function
    K = -dlqr(A, B, Q, R);
    systemLQR = LTISystem('A', A+B*K);
%     absxmax = ones(nx,1)*s_max;
%     absumax = ones(nu,1)*u_max;
%     Xp = Polyhedron('A',[eye(nx); -eye(nx); K; -K], 'b', [absxmax;absxmax; absumax;absumax]);
    Hx=params.constraints.StateMatrix;
    hx=params.constraints.StateRHS;
    Hu=params.constraints.InputMatrix;
    hu=params.constraints.InputRHS;
    Xp = Polyhedron('A',[Hx;Hu*K], 'b', [hx;hu]);
    
    figure(1);
%     Xp.plot(),  alpha(0.25), title('Resulting State Constraints under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3');
    systemLQR.x.with('setConstraint');
    systemLQR.x.setConstraint = Xp;
    InvSetLQR = systemLQR.invariantSet();
    InvSetLQR.plot(), alpha(0.25), title('Invariant Set for Triple Integrator under LQR Control'), xlabel('x_1'), ylabel('x_2'), zlabel('x_3');

    H=InvSetLQR.A
    h=InvSetLQR.b


end