function optimal_RtZ=Kate_Attempt_RunOpt_Eqn13()

clc;clear all;
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;
            dim = size(Z,1)*size(Z,2);

        theta = 0;
        R = [cos(theta), -sin(theta); sin(theta) cos(theta)];
        t = [0 0];
      Ri = repmat(R(:),size(X,1),1);

        RtZ_Ri_guess = [R(:);t(:);Z(:);Ri];


        w = [1;1;1]; %weights

optimal_RtZ =fmincon(@(RtZ_Ri)fun_Eqn13(RtZ_Ri,X,Y,w,dim) ,RtZ_Ri_guess,...
                        [],[],[],[],[],[],@nonlcon);


optimal_Z=reshape((optimal_RtZ(7:7+dim-1)).',[],2)

       figure, show_contour_plot(Z,Y)
       figure, show_contour_plot(optimal_Z,Y)

end


function Ereg=fun_Eqn13(RtZ_Ri,x,y,w,dim)
%Ensure all input matrices have the expected shape
      x=reshape(x',2,[]);
%       y=reshape(y,3,[]);
     %%Unpack the unknown parameters
       R=reshape(RtZ_Ri(1:4),2,2);
       t=RtZ_Ri(5:6);
       Z=reshape((RtZ_Ri(7:(7+dim-1))).',[],2) ;
       Ri = reshape(RtZ_Ri((7+dim):end),[],size(y,1))';



       %%Compute the error terms

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     % Create kd-tree
%     kd = KDTreeSearcher(y);
%       %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%         % kd-tree look-up
%         idz = knnsearch(kd,Z);
%         PyZ = y(idz,:)

        [PyZ,distance,t_a] = distance2curve(y,Z,'spline');

%         NP = NY(idz,:)
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%



       Ematch=w(1)*norm(Z-PyZ,'fro')^2;


           Rxplust=bsxfun(@plus,R*x,t);
       Eprior=w(2)*norm(Z-Rxplust', 'fro')^2;

  for j=1:size(Z,1)

               for i=1:size(Z,1)

                   Eprior2=w(3)*norm((Z(j)-Z(i))-(reshape((Ri(i,:))',2,2) *(x(j)-x(i))),'fro')^2;

               end
  end


     %%Compute total objective
        Ereg=Ematch+Eprior2+Eprior;
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