// Author (C): Samuel GOUGEON - Le Mans, France
// Related Software: Scilab
// License   : CeCILL-B : http://www.cecill.info/licences
// Release   : 1.0
// Date      : 2011-10 -
// Reference : http://bugzilla.scilab.org/show_bug.cgi?id=10110

function [Xc, Yc, a, b] = ellipfit(Xp,Yp,Po)
// ellipfit: Looks for the best circle or ellipse fitting some given 2D data points
//
// ellipfit         : Displays this help
// ellipfit circle  : Runs a randomized circle fitting demo.
// ellipfit ellipse : Runs a randomized ellipse fitting demo.
//     Some difficult cases may fail (without fine initial params)
//
// [xc, yc, r]    = ellipfit(Xp,Yp [,Po]) :
// [xc, yc, a, b] = ellipfit(Xp,Yp [,Po]) :
//   Xp: Row or column of abscissae of points
//   Yp: Row or column or ordinates of points (same length than Xp)
//   xc, yc: Coordinates of the center of the best fitting circle | ellipse
//    r: Radius of the best fitting circle
//    a: Horizontal radius of the best fitting ellipse 
//    b: Vertical radius of the best fitting ellipse 
//   Po: Optionnal vector of initial parameters (best guess). 
//       It may have from 1 to 4 elements, in this order: 
//       [xc0 yc0 r0] or [xc0 yc0 a0] or [xc0 yc0 a0 b0]

    if argn(2)==0	// => help
        head_comments ellipfit
        Xc=0, Yc=0, a=0, b=0
        return
        
    elseif argn(2)>1  // Fitting
        if (size(Xp,1)>1) Xp = matrix(Xp,1,length(Xp)); end
        if (size(Yp,1)>1) Yp = matrix(Yp,1,length(Yp)); end
        if argn(1)==3 then
            [Xc, Yc, R] = rawCircle(Xp,Yp)
            a = R
            b = R
        else
            Xc = mean(Xp)
            Yc = mean(Yp)
            a = strange(Xp)*0.7
            b = strange(Yp)*0.7
        end
        if argn(2)==2 then
            Po = [Xc; Yc; a; b ]
        else
            select length(Po)
            case 1, Po = [ Po; Yc; a ; b ]
            case 2, Po = [ Po(1); Po(2); a ; b ]
            case 3, Po = [ Po(1); Po(2); Po(3) ; Po(3) ]
            case 4, Po = [ Po(1); Po(2); Po(3) ; Po(4) ]
            end
        end
        if argn(1)==3
            [P,res] = datafit(ellipsecarts,[ Xp ; Yp ], Po(1:3));
            Xc = P(1); Yc = P(2); a = P(3); b = a;
        elseif argn(1)==4
            [P,res] = datafit(ellipsecarts,[ Xp ; Yp ], Po);
            Xc = P(1); Yc = P(2); a = P(3); b = P(4);
            if (a>100*stdev(Xp) | b>100*stdev(Yp)) & (~exists("nomore") | exists("nomore") & ~nomore)
                nomore = %t
                [Xc, Yc, a] = ellipfit(Xp,Yp)
                b = a
            end
        else error("Bad number of output parameters")
        end
    // ======  D E M O  ======================================
    else     // ellipfit demo => demo
        disp("ellipfit help   // to see the help"), disp("edit ellipfit  // to see the script")
        // Model (true ellipse):
        t = grand(1,3,"nor",0,3);
        xc = t(1), yc = t(2)
        a = abs(t(3))
        if Xp~="circle"
            b = a*10^grand(1,1,"unf",-0.8,0.8)
            disp(sprintf("Model: xc= %0.2f  yc= %0.2f  a= %0.2f  b= %0.2f",xc,yc,a,b))
            tit = "Fit / model(w/o noise) : a= %.2f(%.2f), b= %.2f(%.2f)   C = [ %.2f(%.2f), %.2f(%.2f) ]"
        else 
            b = a
            disp(sprintf("Model: xc= %0.2f  yc= %0.2f  R= %0.2f",xc,yc,a))
            tit = "Fit / model(w/o noise) : R = %.2f(%.2f)   C = [ %.2f(%.2f), %.2f(%.2f) ]"
        end
        an0 = grand(1,1,"unf",-3,3);
        angles = linspace(an0,an0+grand(1,1,"unf",1.5,4),30);
        xp = xc + a*cos(angles);
        yp = yc + b*sin(angles);

		// Noising the model:
		sig = 0.08*min(a,b);
		xp = xp + sig*rand(xp);
		yp = yp + sig*rand(yp);

		// Fitting parameters:
		[xcF, ycF, aF, bF] = ellipfit(xp,yp)

		// Plotting
		clf
        drawlater
		plot(xp,yp,"b.","markSize",3)
        xarc(xcF-aF,ycF+bF,2*aF,2*bF,0,360*64)
        e = gce(); e.foreground = color("red")
		ax = gca(); ax.isoview = "on";
        if Xp~="circle"
            title(sprintf(tit,aF,a,bF,b,xcF,xc,ycF,yc))
        else
            title(sprintf(tit,aF,a,xcF,xc,ycF,yc))
        end
        drawnow
		Xc=0, Yc=0, a=0, b=0
		return
	end
endfunction
// ----------- U T I L I T I E S  for  ellipfit()  ---------------
function e = ellipsecarts(P,Z)
    // P = [ Xc Yc R ]       ou 
    // P = [Xc Yc a b ]
    a = P(3); 
    b = a;
    tilt = 0;
    if length(P)==4, b = P(4); end
    e = sum(((Z(1,:)-P(1))/a).^2 + ((Z(2,:)-P(2))/b).^2 - 1)
endfunction
// -----------------------------
function [xc,yc,R] = rawCircle(X,Y)
    // Assesses  xc, yc, and R with the 3 most separated points
    dx = strange(X)
    dy = strange(Y)
    switch_XY = %f
    if dx<dy then
        switch_XY = %t
        x2 = X
        X = Y
        Y = x2
    end
    // "dx>dy"
    [xm,k] = min(X); ym = Y(k);
    [xM,k] = max(X); yM = Y(k);
    [x,k] = gsort(abs(X-(xm+xM)/2))
    x2 = X(k($)); 
    y2 = Y(k($)); 
    
    // Segment m-2 :  a1.x + b1.y = 1
    c1 = [xm ym ; x2 y2]\[1 ; 1]   // a1 = c1(1)  b1 = c1(2)
    // Bissectrice : A1.x + B1.y = 1 avec A1 = c.b1 et B1 = -c.a1
    c = 2 / (c1(2)*(xm+x2) - c1(1)*(ym+y2));
    A1 = c*c1(2);  B1 = -c*c1(1);

    // Segment 2-M :  a2.x + b2.y = 1
    c2 = [x2 y2 ; xM yM]\[1 ; 1]   // a2 = c2(1)  b2 = c2(2)
    // Bissectrice : A2.x + B2.y = 1 avec A2 = c.b2 et B2 = -c.a2
    c = 2 / (c2(2)*(x2+xM) - c2(1)*(y2+yM));
    A2 = c*c2(2);  B2 = -c*c2(1);
    
    // Centre = Intersection des 2 bissectrices :
    C = [A1 B1 ; A2 B2]\[1 ; 1]
    xc = C(1)
    yc = C(2)
    R = mean(sqrt((X-xc).^2+(Y-yc).^2))
    if switch_XY, x = xc, xc=yc, yc=x, end
endfunction