% @copyright: Mardavij Roozbehani. All Rights Reserved
% Commercial use is strictly prohibited without permission.
% General Public License applies
%
% Version of August 15,2011
%
% If you use this code, you must reference: 
%
% [1] M. Roozbehani, A. Megretski, and E. Feron. 
% Optimization of Lyapunov Invariants in Verification of Software Systems, 
% IEEE Transactions on Automatic Control, Under Review, 2011.
% Available at: 
% http://arxiv.org/abs/1108.0170 
% and 
% http://web.mit.edu/mardavij/www/Software.html
%
% This code is for verification of generic Mixed-Integer Linear Models (MILMs) of the form
%
% S(F,H,X0,n,nw,nv);
% x[k+1]=F[x[k];w[k];v[k];1];
% s.t H[x[k];w[k];v[k];1]=0;
% 
% where w[k] \in [-1,1]^nw, and v[k] \in {-1,1}^nv, and x[k] \in [-M,M]^n
%
% The initial condition X0 is described through the matrix H0:
% H0[x[0];w[0];v[0];1]=0;
%
% For More information see reference [1] above.


function [objmin,P]=MILMVerifierH0(F,H,H0,nw,M,theta,mu,SOLVER);

% F is the matrix defining the update equation     x[k+1]=F[x[k];w[k];v[k];1];
% H is the matrix constraining the dynamics:       H[x[k];w[k];v[k];1]=0;
% H0 defines the initial condition:                H0[x[0];w[0];v[0];1]=0;
% nw is the dimension of w: w \in [-1,1]^nw
% M is the bound to be establish on the state variables (x): x \in [-M,M]^n
% theta \in [0,\infty) is the decay    rate of the (theta,mu)-Lyapunov invariant:
% mu    \in [0,\infty) is the constant rate of the (theta,mu)-Lyapunov invariant. mu can be made a decision variable as well.
% V(x[k+1])<=theta*V(x[k])-mu
%
% Set Solver = 2 to use SeDuMi. Set Solver = 1 to use LMILab
% Note: we recommend the latest version of SeDuMi. 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h=1e-6; % h is for purturbation of the LMIs for strict feasibility 

[n,N]=size(F);     % n is the state dimension (dimension of x), N=n+nw+nv+1

nv=N-1-n-nw;       % find the dimension of v, i.e., the number of binary variables

[mH,nH]=size(H);   % mH  is the row dimension of H,  i.e., the number of constraints. nH=N
[mH0,nH0]=size(H0);% mH0 is the row dimension of H0, i.e., the number of constraints defining the initial conditions

%%%%%%%%%%%%%%%%%%%Begin Rescaling%%%%%%%%%%%%%%%%%%%%%%%%%%
%we scale x and the corresponding matrices by M: y = x/M \in [-1,1]^n
Fx=F(:,1:n);Fw=F(:,n+1:n+nw);Fv=F(:,n+nw+1:n+nw+nv);F1=F(:,n+nw+nv+1:n+nw+nv+1);
F=[M*Fx Fw Fv F1]/M;
Hx=H(:,1:n);Hw=H(:,n+1:n+nw);Hv=H(:,n+nw+1:n+nw+nv);Hone=H(:,n+nw+nv+1:n+nw+nv+1);
H=[M*Hx Hw Hv Hone]/M;
H0x=H0(:,1:n);H0w=H0(:,n+1:n+nw);H0v=H0(:,n+nw+1:n+nw+nv);H0One=H0(:,n+nw+nv+1:n+nw+nv+1);
H0=[M*H0x H0w H0v H0One]/M;
% after these scalings we have
% y[k+1]=F[y[k];w[k];v[k];1];
% s.t H[y[k];w[k];v[k];1]=0; where F and H are the new scaled matrices and y \in [-1,1]^n
%%%%%%%%%%%%%%%%%%%%END of Rescaling%%%%%%%%%%%%%%%%%%%%%%%%


% Notation Ye=[y;w;v;1];
L5=[zeros(N-1,1);1]';                     % L5*Ye[k]=1;
L4=[zeros(n+nw,nv);eye(nv);zeros(1,nv)]'; % L4*Ye[k]=v[k]
L3=[eye(n+nw);zeros(nv+1,n+nw)]';         % L3*Ye[k]=[y[k];w[k]]
L2=[eye(n) zeros(n,N-n);zeros(1,N-1) 1];  % L2*Ye[k]=y[k]
L1=[F;L5];                                % L1*Ye[k]=y[k+1]

Q = diag([1*ones(1,n) -1]); % We want to prove that y'*Q*y <=1, equivalently: x'*Q*x <= M^2
                            % Choose a different Q if you need to
                            % For instance, Q_i = [e_i -1], proves
                            % y_i^2<=M^2 provided that for every Q_i a
                            % solution is found. See [1] for more info
                            
                            
% V(y) = [y;1]'*P*[y;1]=Ye'*L2'*P*L2*Ye
% V(y+)=[x+/M;1]'*P*[x+/M;1]=Ye'*L1'*P*L1*Ye
% |y|'*|y|-1<=V(y): ---> Ye.'*L2'*Q*L2*Ye <= Ye'*L2'*P*L2*Ye
  
yalmip('clear');

% Select the Solver
if SOLVER == 1

    TOL=1e-6;
    FRAD=1e9;
    MAXIT=20;
    options = sdpsettings('solver','lmilab');
    options.lmilab.reltol=TOL;
    options.lmilab.feasradius=FRAD;
    options.lmilab.L=MAXIT;

elseif SOLVER == 2

    options = sdpsettings('solver','sedumi');
    options.sedumi.numtol=1e-12;
    options.sedumi.eps=1e-10;
    options.sedumi.bigeps=1e-8;
    options.sedumi.bignumtol=1e-6;
    options.sedumi.alg=2;

end

t = sdpvar(1,1,'symmetric','real');
q = sdpvar(1,1,'symmetric','real');       % scaling variable, set q=1 as default
P = sdpvar(n+1,n+1,'symmetric','real');   % V(y) = [y;1]'*P*[y;1]=Ye'*L2'*P*L2*Ye
Dxw=sdpvar(n+nw,n+nw,'diagonal','real');  % multiplier for x and w
D2xw=sdpvar(n+nw,n+nw,'diagonal','real'); % multiplier for x and w
D0xw=sdpvar(n+nw,n+nw,'diagonal','real'); % multiplier for x and w
Dv=sdpvar(nv,nv,'diagonal','real');       % multiplier for v
D2v=sdpvar(nv,nv,'diagonal','real');      % multiplier for v
D0v=sdpvar(nv,nv,'diagonal','real');      % multiplier for v
Y = sdpvar(N,mH,'full','real');           % multiplier for H*Ye=0
Y2 = sdpvar(N,mH,'full','real');          % multiplier for H*Ye=0  
Y0 = sdpvar(N,mH0,'full','real');         % multiplier for H0*Ye=0

MM{1} = L1'*P*L1-theta*L2'*P*L2-(Y*H+H'*Y')-L3'*Dxw*L3-L4'*Dv*L4+(trace(Dxw)+trace(Dv)+mu)*L5'*L5; % V(y[k+1])=[x[k]/M;1]'*P*[x[k]/M;1]=Ye'*L1'*P*L1*Ye<=theta*V(y[k])-mu=Ye'*L2'*P*L2*Ye-mu  
                                                                                                   % subject to H*Ye=0, y, w \in [-1,1]^(n+nw), and v in {-1,1}^nv
MM{2} = q*L1'*Q*L1-L2'*P*L2-(Y2*H+H'*Y2')-L3'*D2xw*L3-L4'*D2v*L4+(trace(D2xw)+trace(D2v))*L5'*L5;  % |y[k+1]|'*|y[k+1]|-1<=V(y): ---> Ye.'*L1'*Q*L1*Ye <= Ye'*L2'*P*L2*Ye
                                                                                                   % subject to H*Ye=0, y, w \in [-1,1]^(n+nw), and v in {-1,1}^nv
                                                                                                   % q>0 is for proper scaling. It helps find a solution when the feasible cone is too "narrow". Set q=1 as default
MM{3} = -Dxw;                                                                                      % non-negative multipliers
MM{4} = -D2xw;                                                                                     % non-negative multipliers
MM{5} = -D0xw;                                                                                     % non-negative multipliers
MM{6} = L2'*P*L2-(Y0*H0+H0'*Y0')-L3'*D0xw*L3-L4'*D0v*L4+(trace(D0xw)+trace(D0v))*L5'*L5;           % V(y[0]) <=0 for all y(0) where H0[y(0);w(0);v(0);1]=0;


% Setting up the LMIs (as a feasibility problem) in yalmip. 
FF = set(t-h<0)+set(q>1e-3);
FF = FF + set(MM{1} + h*eye(N)    < t*eye(N),'negtaive definite');
FF = FF + set(MM{2} + h*eye(N)    < t*eye(N),'negtaive definite');
FF = FF + set(MM{3} + h*eye(n+nw) < t*eye(n+nw),'negtaive definite');
FF = FF + set(MM{4} + h*eye(n+nw) < t*eye(n+nw),'negtaive definite');
FF = FF + set(MM{5} + h*eye(n+nw) < t*eye(n+nw),'negtaive definite');
FF = FF + set(MM{6} + h*eye(N)    < t*eye(N),'negtaive definite');
% end of setting up the LMIs

% Solving the LMIs
sol=solvesdp(FF,[],options);

% Getting the solutions
P=double(P);
objmin=double(t)-h;

for I=1:6
    MM{I}=double(MM{I});
    tt(I)=max(eig(MM{I}));
end


if max(tt)<0,
    fprintf(' correctness established: objmin=%f\n',objmin)
else
    if objmin<0,
        fprintf(' NUMERICAL DIFFICULTIES\n')
        disp(max(tt))
        disp(objmin)
    else
        fprintf(' no invariants found: objmin=%f\n', objmin)
    end
end