%----------------------------------
%  MATLAB code for CS_+ hierarchy
%  Corresponding to paper: arXiv:1506.08810
%  This code tests whether the matrix M satisfies
%  the constraints defined by the 3rd level of the SDP
%  hierarchy defined in arXiv:1506.08810.
%----------------------------------

N = 5; % size of the matrix

% The program only works for n = 3
n = 3; % level of the hierarchy 


%
% The matrix K is known not to be in CS_+
%
K = [4 0 2 2 0; 0 4 0 2 2; 2 0 4 0 3; 2 2 0 4 0; 0 2 3 0 4];
 
%
% Input matrix
%
M = K;

assert(n == 3, 'n has to be equal to 3');

delta = (N^(n+1)-1)/(N-1);  % Size of the matrix Omega
deltap = (N^((n-1)/2+1)-1)/(N-1); % Size of the small matrices

% Computing the constraints matrices

%
% The cyclicity constraints
%
C = spalloc(delta^2,(N+1)^6,2*(N+1)^6);
cnt = 1;
tic
for a=1:(1+N)
    wa = ind2word(N,a);
    for b=1:(1+N)
        wb = ind2word(N,b);
        for c=1:(1+N)
            wc = ind2word(N,c);
            for d=1:(1+N)
                wd = ind2word(N,d);
                for e=1:(1+N)
                    we = ind2word(N,e);
                    for f=1:(1+N)
                        wf = ind2word(N,f);
                        abc = word2ind(N, [wa wb wc]);
                        def = word2ind(N, [wd we wf]);
                        bcf = word2ind(N, [wb wc wf]);
                        ade = word2ind(N, [wa wd we]);
                        if abc == bcf && def == ade
                            continue;
                        else
                            C(sub2ind([delta delta],abc,def),cnt) = 1;
                            C(sub2ind([delta delta],bcf,ade),cnt) = -1;
                            cnt = cnt+1;
                        end
                    end
                end
            end
        end
    end
end
C = C';

%
% The positivity constraints
%
Omegap_ind = zeros(deltap^4,(1+N)^2);
for alpha=1:(1+N)
    wa = ind2word(N,alpha);
    for beta=1:(1+N)
        wb = ind2word(N,beta);
        for r=1:deltap
            wr = ind2word(N,r);
            for s=1:deltap
                ws = ind2word(N,s);
                for u=1:deltap
                    wu = ind2word(N,u);
                    for v=1:deltap
                        wv = ind2word(N,v);
                        ras_ind = word2ind(N, [wr wa ws]);
                        ubv_ind = word2ind(N, [wu wb wv]);
                        ab_ind = sub2ind([1+N 1+N], alpha, beta);
                        ru_ind = sub2ind([deltap deltap], r, u);
                        sv_ind = sub2ind([deltap deltap], s, v);
                        Omegap_ind(sub2ind([deltap^2 deltap^2],ru_ind,sv_ind),ab_ind) = sub2ind([delta delta],ras_ind,ubv_ind);
                    end
                end
            end
        end
    end
end





cvx_solver sedumi
cvx_precision medium

cvx_begin sdp
    variable Omega(delta,delta) semidefinite 
    maximize 0
    subject to

        Omega(1,1) == 1;
        
        % For words of length 1, Omega is equal to M
        Omega(2:(1+N),2:(1+N)) == M;        
                
        % Positivity conditions
        for ii=1:(1+N)^2
            reshape(Omega(Omegap_ind(:,ii)),deltap^2,deltap^2) == semidefinite(deltap^2,0);
        end
     
        % Cyclicity of the trace condition
        C*Omega(:) == 0;
        
cvx_end

%
if cvx_optval == 0
    fprintf('The matrix M is maybe in CS_+\n');
elseif cvx_optval == -Inf
    fprintf('The matrix M is (numerically) not in CS_+ \n'); 
end

