%% SPE-knn
% SPE with nearest neighbor constraints -- uses a working set method


function [Y, K, eigVals] = spe(A, tol, targetd, verbose)


N = length(A);

k = sum(A, 2);
neighborMethod = 'bdmatch';
T = 10^10;
watchCuts = [];


K = A * A';
K0 = K;


cutDiff = Inf;
cti = 0;


Cs = {};
bs = {};
slackInd = {};

% %%% add tr less then 1 constraint
% C1 = eye(N);
% Cs{c} = C1;
% bs{c} = 1;
% slackInd{c} = 1;


%%% add centering constraint
c = 1;
C1 = ones(N);
Cs{c} = C1;
bs{c} = 0;
slackInd{c} = 0;

[idx, jdx] = find(triu(A));

for i=1:length(idx)
    c = c+1; 
    ii = idx(i);
    jj = jdx(i);
    
    C1 = zeros(N,N);
    C1(ii, ii) = 1;
    C1(jj, jj) = 1;
    C1(ii, jj) = -1;
    C1(jj, ii) = -1;
    
    Cs{c} = C1;
    bs{c} = 1; %default all connected distances have to be less then one
    slackInd{c} = 1;
end



[idx, jdx] = find(triu(1 - (A + eye(N, N))));

AA2 = zeros(length(idx), N*N);
for i=1:length(idx) 
    ii = idx(i);
    jj = jdx(i);
    
    C1 = zeros(N,N);
    C1(ii, ii) = 1;
    C1(jj, jj) = 1;
    C1(ii, jj) = -1;
    C1(jj, ii) = -1; 
    AA2(i, :) = C1(:);   
end

numViolations = 1;
while numViolations > 0
   
    %%% Set up constraint matrices
    disp(sprintf('Building constraint matrices...'));
    AA = zeros(c, (N+c)*(N+c));
    bb = zeros(c, 1);
    cc = zeros((N+c)*(N+c), 1);

    %%% add constraints
    for i=1:c
        C = Cs{i};

        Cpad = zeros(N+c, N+c);
        Cpad(1:N, 1:N) = C;
        Cpad(N+i, N+i) = slackInd{i};

        AA(i, :) =  Cpad(:);
        bb(i, :) = bs{i};

        Apad = zeros(N+c, N+c);
        %Apad(1:N, 1:N) = A;
        Apad(1:N, 1:N) = eye(N, N);

        cc = -1*Apad(:);
    end

    flags.s=N+c;
    flags.l=0;
    OPTIONS.maxiter = 50;

    %%% solve sdp
    [x d z info]=csdp(AA,bb,cc,flags,OPTIONS);
    K = reshape(x, N+c, N+c);
    K = K(1:N, 1:N);
    
    %%% check violations
    newIDX = find(AA2 * K(:) < 1);
    
    [Y, eigVals] = eigDecomp(K);
    
    if verbose     
        plotEmbedding(Y, A, 'SPE cuts in progress', 1);
    end
        
    numViolations = length(newIDX);
    
    disp(sprintf('Addding %i constraints', numViolations));
    
    for i=1:numViolations
        c = c+1;
        ii = idx(newIDX(i));
        jj = jdx(newIDX(i));

        C1 = zeros(N,N);
        C1(ii, ii) = 1;
        C1(jj, jj) = 1;
        C1(ii, jj) = -1;
        C1(jj, ii) = -1; 
        
        Cs{c} = C1;
        bs{c} = 1; %default all unconnected distances have to be greater then one
        slackInd{c} = -1;

    end

end

%Spectral embedding
[Y, eigVals] = eigDecomp(K);

eigNorm = eigVals ./ sum(eigVals);
score = 0;
Y;
eigVals;
K;


function [Y, eigV] = eigDecomp(K)
    [V, D]=eig(K);
    D0 = diag(D);
    V = V * sqrt(D);
    Y=(V(:,end:-1:1))';
    eigV=D0(end:-1:1);
    
    [eigV, IDX] = sort(eigV, 'descend');
    Y = Y(IDX, :);

% Plots a 2d embedding
function plotEmbedding(Y, neighbors, plotTitle, figureNum)
    figure(figureNum);
    clf;
    
    N = length(neighbors);
    
    scatter(Y(1,:),Y(2,:), 60,'filled'); axis equal;
    for i=1:N
        for j=1:N
            if neighbors(i, j) == 1
                line( [Y(1, i), Y(1, j)], [ Y(2, i), Y(2, j)], 'Color', [0, 0, 1], 'LineWidth', 1);
            end
        end
    end
    
    title(plotTitle);
    drawnow; 
    axis off;
    