function XYproj = ProjectPointsOntoConicByEberlyModified(XY,ParA)
%
%  The modified Eberly's method  (using agebraic parameters of the conic)
%
%   Projecting a given set of points onto a quadratic curve (conic)

%   The conic is defined by quadratic equation 
%      Ax^2 + 2Bxy + Cy^2 + 2Dx + 2Ey + F = 0

%   Input:  A = (A,B,C,D,E,F)' is the column vector of the parameters of the conic
%           XY(n,2) is the array of coordinates of n points x(i)=XY(i,1), y(i)=XY(i,2)

%   Output:  XYproj is the array of coordinates of projections
%
%        Nikolai Chernov,  August 2012

n = size(XY,1);         %  n is the number of points to be projected
XYproj = zeros(n,2);    %  prepare an array of projections

M22 = [ParA(1) ParA(2);      %  2x2 matrix of main parameters
       ParA(2) ParA(3)];
   
[Q D] = eig(M22);            %  eigendecomposition

%    Next ensure the condition  abs(D(1,1)) <= abs(D(2,2))
%    (if not, inerchange the variables x and y

if abs(D(1,1)) > abs(D(2,2))
    [D(1,1) D(2,2)] = deal(D(2,2),D(1,1));
    [Q(:,1) Q(:,2)] = deal(Q(:,2),Q(:,1));
end

%   Next compute the new parameters A1, A3, A4i, A5i, A6i
%   in the rotated coordinates system (where A2=0)
%    A4i, A5i, A6i mean the "initial values" to be changed later

A3 = abs(D(2,2));     %   A3 is always non-negative
if D(2,2)>=0
    A1 = D(1,1); A4i = Q(1,1)*ParA(4)+Q(2,1)*ParA(5); A5i = Q(1,2)*ParA(4)+Q(2,2)*ParA(5); A6i = ParA(6);
else
    A1 =-D(1,1); A4i =-Q(1,1)*ParA(4)-Q(2,1)*ParA(5); A5i =-Q(1,2)*ParA(4)-Q(2,2)*ParA(5); A6i =-ParA(6);
end

UV = XY*Q;    %      Rotate the data point to the new coordinates

iterMax = 100; %   Maximum number of Newton's ietrations. Usually, 4-6 are enough

for i=1:n        %  Main loop over the points to be projected

    u = UV(i,1);   v = UV(i,2);    %  the current point

%       Next we translate the coordinate system so that the current
%       point (u,v) moves to the origin, i.e., it becomes (0,0)
%       Accordingy we modify A4, A5, A6.
    
    A4 = A4i + A1*u;
    A5 = A5i + A3*v;
    A6 = A6i + (A1*u + 2*A4i)*u + (A3*v + 2*A5i)*v;

%      Next we negate the x-coordinate if needed
%      to ensure that A4>=0  (for our convenience)
    
    sign_x = 1;
    if A4<0
        A4 = -A4;
        sign_x = -1;
    end

%      Next we negate the y-coordinate if needed
%      to ensure that A5>=0  (for our convenience)

    sign_y = 1;
    if A5<0
        A5 = -A5;
        sign_y = -1;
    end
    
    if (A3-A1)^2 == 0
        [xp,yp] = CircleCase(A3,A4,A5,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end
    
    if A4^2 == 0     % degenerate case: point on the minor axis
        [xp,yp] = PointOnMinorAxis(A1,A3,A5,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end
   
    if A5^2 == 0     % degenerate case: point on the major axis
        [xp,yp] = PointOnMajorAxis(A1,A3,A4,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end
    
%          Next we choose an initial value of T
%              (T is an auxiliary variable)

    if A1 < 0                 %  Hyperbola
        CE=sqrt(A5*sqrt(A3));
        AD=sqrt(A4*sqrt(-A1));
        Finf=(AD-CE)/(A3-A1)^2*(A4^2*(2*A3-A1-A1*CE/AD)/AD+A5^2*(A3-2*A1+A3*AD/CE)/CE)+A6;
    end
    
%      Finf is the value of F at the inflection point

    if (A1 < 0) & (Finf > 0)
        signF = -1;
        if A6 <= 0
            T = 0;
        else
            T = A6/(A4^2-A1*A6+sqrt(A4^2-A1*A6)*A4);
        end
    else
        signF = 1;
        if A6 >= 0
            T = 0;
        else if A1 < 0
                T = A6/(A5^2-A3*A6+sqrt(A5^2-A3*A6)*A5);
            else
                den1 = A4^2*(1-A1*A6/A4^2+sqrt(1-A1*A6/A4^2));
                den2 = A5^2*(1-A3*A6/A5^2+sqrt(1-A3*A6/A5^2));
                T = A6/max(den1,den2);
            end
        end
    end

    if A3*T+1 <= 0     % degenerate case: point on the major axis
        [xp,yp] = PointOnMajorAxis(A1,A3,A4,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end
    
    if A1*T+1 <= 0     % degenerate case: point on the minor axis
        [xp,yp] = PointOnMinorAxis(A1,A3,A5,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end

    %  the main, non-singular case
    %   start Newton's iterations.

    proj_found = 0;

    for iter=1:iterMax    %    loop of Newton's iterations to solve F(T)=0
        
        AT1 = A1*T+1;  AT2 = A3*T+1;
        if ((AT1<=0)||(AT2<=0)),  break,  end;
        AA1 = (A4/AT1)^2;  AA2 = (A5/AT2)^2;
        
        F = -T*(AA1*(AT1+1) + AA2*(AT2+1)) + A6;
        %  value of F; we need to find T such that F(T)=0
        if signF*F < 0               %   gone too far, emergency stop
            if iter == 1   %  started on the wrong side (hidden singularity)
                if A4 > A5
                    [xp,yp] = PointOnMajorAxis(A1,A3,A4,A6);
                else
                    [xp,yp] = PointOnMinorAxis(A1,A3,A5,A6);
                end
                XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
                proj_found = 1;
            end
            break;
        end
        Tnew = T + F/2/(AA1/AT1 + AA2/AT2);
        if ((~isfinite(Tnew))||(T == Tnew)),  break,  end;   %  no progress, terminate iterations
        T = Tnew;                   %  Newton's iteration
    end    %  end Newton's iterations

    if proj_found, continue, end;
    
    xprojx = -T*A4/AT1;
    qqqx = (A1*xprojx+2*A4)*xprojx+A6;
    discx = A5^2-A3*qqqx;
    if discx > 0
        yprojx = -qqqx/(A5+sqrt(discx));
        Fx = xprojx^2 + yprojx^2;        % "quality" of first candidate
    else
        Fx = realmax;
    end

    yprojy = -T*A5/AT2;
    qqqy = (A3*yprojy+2*A5)*yprojy+A6;
    discy = A4^2-A1*qqqy;
    if discy > 0
        xprojy = -qqqy/(A4+sqrt(discy));
        Fy = xprojy^2 + yprojy^2;         % "quality" of second candidate
    else
        Fy = realmax;
    end

    if (Fx==realmax)&&(Fy==realmax)
        [xp,yp] = CircleCase(A3,A4,A5,A6);
        XYproj(i,:) = UV(i,:) + [sign_x*xp sign_y*yp];
        continue;
    end

    if Fx < Fy         %    the first  candidate is better
        XYproj(i,:) = UV(i,:) + [sign_x*xprojx sign_y*yprojx];
    else               %    the second candidate is better
        XYproj(i,:) = UV(i,:) + [sign_x*xprojy sign_y*yprojy];
    end                %    end comparing the two candidates
end

for i=1:n
    if ((~isfinite(XYproj(i,1)))||(~isfinite(XYproj(i,2))))
        XYproj(i,:) = UV(i,:);
    end
end
XYproj = XYproj*Q';     %  final rotation

end     %  function ProjectPointsOntoConicByEberlyModified

function [xp,yp] = PointOnMajorAxis(A1,A3,A4,A6)

xp = A4/(A3-A1);
polynom = (A1*xp+2*A4)*xp+A6;
if polynom <= 0
    yp = sqrt(-polynom/A3);
else
    xp = -A6/(A4+sqrt(A4^2-A1*A6));
    yp = 0;
end
end     %  function PointOnMajorAxis

function [xp,yp] = PointOnMinorAxis(A1,A3,A5,A6)

if A1 >= 0
    xp = 0;
    yp = -A6/(A5+sqrt(A5^2-A3*A6));
else
    yp = A5/(A1-A3);
    polynom = (A3*yp+2*A5)*yp+A6;
    if polynom <= 0
        xp = sqrt(-polynom/A1);
    else
        xp = 0;
        yp = -A6/(A5+sqrt(A5^2-A3*A6));
    end
end
end     %  function PointOnMinorAxis

function [xp,yp] = CircleCase(A3,A4,A5,A6)

AA = A4^2 + A5^2;
if AA == 0
    xp = sqrt(-A6/A3);
    yp = 0;
else
    Frac = -A6/(1 + sqrt(1-A3*A6/AA))/AA;
    xp = A4*Frac;
    yp = A5*Frac;
end
end     %  function CircleCase