% Copyright (c) Wei Lian (2010,2011), Department of computer science, Changzhi University, Changzhi, Shanxi province, China
clear

lambda=10;mu=1;

X=load('Xpoint_set.txt');Y=load('Ypoint_set.txt');
X=TSP_greedy(X);
X=choose_refer_and_recirc(X,'close');
Nx=size(X,1);Ny=size(Y,1);

scdist=SC_cost_fast_fun([(1:Nx)' ones(Nx,1)],X,Y,50);

dify=repmat(permute(Y,[1,3,2]),[1,Ny,1])-repmat(permute(Y,[3,1,2]),[Ny,1,1]);
leny=sqrt(sum(dify.^2,3));

prev=zeros(Ny,Ny,Nx);

lenxneig=sqrt(sumsqr(X(2,:)-X(1,:)));
energy=lambda*abs(leny-lenxneig)+scdist(:,:,2);

for ii=3:Nx        
    lenxneig=sqrt(sumsqr(X(ii,:)-X(ii-1,:)));
    lenxbase=sqrt(sumsqr(X(ii,:)-X(1,:)));
    
    [energy2, index]=DP_SC_fan_rotate_speedup( energy, leny, lenxneig, lambda);
    prev(:,:,ii)=index; 

    energy=energy2+scdist(:,:,ii)+mu*(leny-lenxbase).^2./(leny+lenxbase+eps);
end

corr=zeros(Nx,1);
enmin=min(energy(:));
[indx,indy]=find(energy==enmin);

prev=permute(prev(:,indy(1),:),[1,3,2]);
corr(1)=indy(1);
corr(Nx)=indx(1);
for ii=Nx-1:-1:2
    corr(ii)=prev(corr(ii+1),ii+1);  
end

P=zeros(Nx,Ny);
for ii=1:Nx
    P(ii,corr(ii))=1;
end

Xe=[X ones(Nx,1)];
Xtran=Xe*inv(Xe'*Xe)*(Xe'*P*Y);
figure;hold on;
plot(Y(:,1),Y(:,2),'b+','LineWidth',2);
plot(Xtran(:,1),Xtran(:,2),'r*','LineWidth',2);
plot([Xtran(:,1) P*Y(:,1)]',[Xtran(:,2),P*Y(:,2)]','k-','LineWidth',2);
