function run_LS()
clc;clear all;
close all
  beep off


nit=1200

     w3 = 1.0;
    w4 = 1000000;




X = [0.326118759777820,-0.277140616245993;0.437080391770018,-0.0558590878401746;0.548042023762216,0.162592814787714;0.659003655754414,0.375821686878482;0.769965287746612,0.581491348288492;]
Y = [-0.698446913206480,-0.715661868085998;-0.625305502069696,-0.444146525051125;-0.503232714874489,-0.197186449680767;-0.475138257846766,0.126591717108409;-0.438086730813650,0.443496979840961;]
     [X Y] = mean_center_and_normalize(X,Y);
    Z = X;
    M1 = sparse(size(Z,1), size(Z,1));
    M1 = spdiags(-ones(size(Z,1),1),0,M1);
    M1 = spdiags(ones(size(Z,1),1),1,M1);
    M1(end,1) = 1;
    M2 = sparse(size(Z,1), size(Z,1));
    M2 = spdiags(-ones(size(Z,1),1),0,M2);
    M2 = spdiags(ones(size(Z,1),1),2,M2);
    M2(end-1,1) = 1;
    M2(end,2) = 1;
    M = [M1;M1';M2;M2'];
    Ex = M*X;
       figure;
       axis off;
    axis square;
     show_contour_plot(X,Y);
    NY = compute_normal(Y);


stopnow=false;

 type = 2
    w1 = 1;
    w2 = 0.1;
    Zo = Z;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Create kd-tree
    kd = KDTreeSearcher(Y);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Initialize linear system ||D^0.5(Av - b)||_2^2 + ||W^0.5v||_2^2
    dim = size(Z,1)*size(Z,2);
    A = sparse(size(Z,1)+6*dim, dim+size(Z,1)+3);
    A((1+size(Z,1)):(size(Z,1)+dim),1:dim) = speye(dim,dim);
    A((1+size(Z,1)+dim):(size(Z,1)+2*dim),1:dim) = speye(dim,dim);
    A((1+size(Z,1)+dim):(size(Z,1)+dim+dim/2), dim+2) = -ones(dim/2,1);
    A((1+size(Z,1)+dim+dim/2):(size(Z,1)+2*dim), dim+3) = -ones(dim/2,1);
    M1 = sparse(size(Z,1), size(Z,1));
    M1 = spdiags(-ones(size(Z,1),1),0,M1);
    M1 = spdiags(ones(size(Z,1),1),1,M1);
    M1(end,1) = 1;
    M2 = sparse(size(Z,1), size(Z,1));
    M2 = spdiags(-ones(size(Z,1),1),0,M2);
    M2 = spdiags(ones(size(Z,1),1),2,M2);
    M2(end-1,1) = 1;
    M2(end,2) = 1;
    M = [M1;M1';M2;M2'];
    A((size(Z,1)+2*dim+1):(size(Z,1)+4*dim),1:size(Z,1)) = M;
    A((size(Z,1)+4*dim+1):end,(size(Z,1)+1):dim) = M;
    b = zeros(size(Z,1)+6*dim,1);
    D = sparse(size(Z,1)+6*dim, size(Z,1)+6*dim);
    D(1:size(Z,1),1:size(Z,1)) = w1*speye(size(Z,1),size(Z,1));
    D((size(Z,1)+1):(size(Z,1)+dim),(size(Z,1)+1):(size(Z,1)+dim)) = w2*speye(dim,dim);
    W = sparse(dim+size(Z,1)+3, dim+size(Z,1)+3);
    W((dim+1):(dim+3), (dim+1):(dim+3)) = 0.1*speye(3, 3);
    W((dim+4):end,(dim+4):end) = 1*speye(size(Z,1), size(Z,1));
    for it=1:nit
        if(stopnow)
            return;
        end;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % kd-tree look-up
        idz = knnsearch(kd,Z);
        P = Y(idz,:);
        NP = NY(idz,:);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Build linear system
        N = [spdiags(NP(:,1),0,size(NP,1),size(NP,1)), ...
             spdiags(NP(:,2),0,size(NP,1),size(NP,1))];
        A(1:size(Z,1),1:dim) = N;
        Xr = X;
        Xr(:,1) = -Xr(:,1);
        Xr = fliplr(Xr);
        A((1+size(Z,1)+dim):(size(Z,1)+2*dim),dim+1) = reshape(Xr,dim,1);
        A((size(Z,1)+2*dim+1):(3*dim),(dim+4:end)) = spdiags(Ex(1:size(Z,1),2),0,size(Z,1),size(Z,1));
        A((3*dim+1):(size(Z,1)+3*dim),(dim+4:end)) = spdiags(Ex((1+size(Z,1)):(2*size(Z,1)),2),0,size(Z,1),size(Z,1));
        A((size(Z,1)+3*dim+1):(4*dim),(dim+4:end)) = spdiags(Ex((1+2*size(Z,1)):(3*size(Z,1)),2),0,size(Z,1),size(Z,1));
        A((4*dim+1):(size(Z,1)+4*dim),(dim+4:end)) = spdiags(Ex((1+3*size(Z,1)):end,2),0,size(Z,1),size(Z,1));
        A((size(Z,1)+4*dim+1):(5*dim),(dim+4:end)) = spdiags(-Ex(1:size(Z,1),1),0,size(Z,1),size(Z,1));
        A((5*dim+1):(size(Z,1)+5*dim),(dim+4:end)) = spdiags(-Ex((1+size(Z,1)):(2*size(Z,1)),1),0,size(Z,1),size(Z,1));
        A((size(Z,1)+5*dim+1):(6*dim),(dim+4:end)) = spdiags(-Ex((1+2*size(Z,1)):(3*size(Z,1)),1),0,size(Z,1),size(Z,1));
        A((6*dim+1):end,(dim+4:end)) = spdiags(-Ex((1+3*size(Z,1)):end,1),0,size(Z,1),size(Z,1));
        b(1:size(Z,1)) = N*reshape(P,dim,1);
        b((1+size(Z,1)):(size(Z,1)+dim)) = reshape(P,dim,1);
        b((1+size(Z,1)+dim):(size(Z,1)+2*dim)) = reshape(X,dim,1);
        b((size(Z,1)+2*dim+1):end) = reshape(Ex,dim*4,1);
        D((size(Z,1)+dim+1):(size(Z,1)+2*dim),(size(Z,1)+dim+1):(size(Z,1)+2*dim)) = w3*speye(dim,dim);
        D((size(Z,1)+2*dim+1):end,(size(Z,1)+2*dim+1):end) = w4*speye(dim*4,dim*4);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Solve
        v = (A'*D*A + W)\(A'*D*b);
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Extract solution
        Z = reshape(v(1:dim), size(X,1), size(X,2));
        theta = v(dim+1);
        R = [cos(theta) -sin(theta); sin(theta) cos(theta)];
        X = X*R' + repmat(v((dim+2):(dim+3))', [size(X,1),1]);
        for i=1:size(Z,1)
            theta = v(dim+3+i);

            R = [cos(theta) -sin(theta); sin(theta) cos(theta)];
            Ex(i,:) = Ex(i,:)*R';
            Ex(i+size(Z,1),:) = Ex(i+size(Z,1),:)*R';
            Ex(i+2*size(Z,1),:) = Ex(i+2*size(Z,1),:)*R';
            Ex(i+3*size(Z,1),:) = Ex(i+3*size(Z,1),:)*R';
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     % Stopping Criteria
        if(norm(Z-Zo)/size(Z,1) < 1e-6 && type == 1)
            return
        elseif(norm(Z-Zo)/size(Z,1) < 1e-4 && type == 2)
            w3 = w3*0.1;
            w4 = w4*0.8;
        end;
        Zo = Z;
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    end
        SS=full(A)
    disp('p')
       % Show result
      figure;
      axis off;
    axis square;
        show_contour_plot(Z,Y)

end


function NY = compute_normal(Y)
    R = [0 -1;1 0];
    Y = Y*R';
    N1 = circshift(Y,1)-Y;
    n = sqrt(sum(N1.^2,2));
    N1(:,1) = N1(:,1)./n;
    N1(:,2) = N1(:,2)./n;
    N2 = Y-circshift(Y,-1);
    n = sqrt(sum(N2.^2,2));
    N2(:,1) = N2(:,1)./n;
    N2(:,2) = N2(:,2)./n;
    NY = (N1+N2)./2;
    n = sqrt(sum(NY.^2,2));
    NY(:,1) = NY(:,1)./n;
    NY(:,2) = NY(:,2)./n;
end


function show_contour_plot(X,Y)

    X(end+1,:) = X(1,:);
    Y(end+1,:) = Y(1,:);
    plot(X(:,1), X(:,2), '.', 'color',[ 0  0 .9]);   hold on;
    plot(Y(:,1), Y(:,2), 'x', 'color',[.5 .5  0]);   hold on;
    drawnow;
end

function [X Y] = mean_center_and_normalize(X,Y)
    XnY = [X;Y];
    avg = mean(XnY);
    X = X - repmat(avg, [size(X,1),1]);
    Y = Y - repmat(avg, [size(Y,1),1]);
    XnY = [X;Y];
    d = sqrt(max(sum(XnY.^2,2)));
    X = X./d;
    Y = Y./d;
end

 function [c,ceq]=nonlcon(params)
    %Ensure R is a rotation matrix (orthogonal with determinant=1)
      R=reshape(params(1:4),2,2);
      v=R*R'-eye(2);
      ceq(5,1) = det(R)-1;
      ceq(1:4)= v(:);
      c=[];
    end