%% 2d example
%% Generate some data
close all
clear
clc
N=50; %# points
n=2; %dim
nc=2; %# clusters
x=zeros(n,N);
means=[-0.05 0.05];
sig=0.05;
i=1;
J=zeros(N*N,N);
for t=1:N
    for k=t+1:N
        J(i,t)=1;J(i,k)=-1;
        i=i+1;
    end
end

for t=1:N/nc
    ang=rand(2,1)*2*pi;
    x(:,t)=means(:,1)+sig*rand*[cos(ang(1));sin(ang(1))];
    x(:,t+N/nc)=means(:,2)+sig*rand*[cos(ang(2));sin(ang(2))];
end

figure(1)
scatter(x(1,1:N/2),x(2,1:N/2),'r')
hold on
scatter(x(1,N/2+1:end),x(2,N/2+1:end),'b')


%% son clustering
lambda =0.001;
cvx_begin quiet
variables X(n,N)
minimize( sum(sum((x-X).*(x-X)))+lambda*sum(norms(J*X',2,2)) )
cvx_end


%% plotting
color={[1 0 0],[0 0 1],[0 1 0],[1 0 1]};
hold on
eps=0.01;
ind=zeros(N,1);
ind(1)=1;
m=1;
plot(x(1,1),x(2,1),'color',color{m},'marker','x')
for k=1:N
    if ind(k)==0
        ind(k)=m;
        plot(x(1,k),x(2,k),'color',color{m},'marker','x')
        for t=k+1:N
            if (X(:,k)-X(:,t))'*(X(:,k)-X(:,t))<eps
                ind(t)=m;
                plot(x(1,t),x(2,t),'color',color{m},'marker','x')
            end
        end
        m=m+1;
    end
end
