// Author (C): Samuel GOUGEON - Le Mans, France
// Related Software: Scilab
// License   : CeCILL-B : http://www.cecill.info/licences
// Release   : 2.0
// Date      : 2015-05-12
// Repository: http://fileexchange.scilab.org/toolboxes/194000
// Reference : http://bugzilla.scilab.org/10110

function [Xc, Yc, a, b, g] = ellipfit(Xp,Yp,Po)
// ellipfit: Looks for the best circle or ellipse fitting some given 2D data points
//
// CALLING SEQUENCES:
// ellipfit             // Displays this help
// ellipfit circle      // Runs a randomized circle-fitting demo.
// ellipfit ellipse     // Runs a randomized ellipse-fitting demo.
//
// [xc, yc, r]       = ellipfit(Xp, Yp [,Po]) // fits data with a circle
// [xc, yc, a, b]    = ellipfit(Xp, Yp [,Po]) // fits data with a right ellipse
// [xc, yc, a, b, g] = ellipfit(Xp, Yp [,Po]) // fits data with a tilted ellipse
//
//  PARAMETERS:
//   Xp: Row or column of abscissae of data points
//   Yp: Row or column or ordinates of data 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 
//    g: Tilt angle [°], on [0, 90]
//   Po: Optionnal vector of initial parameters (best guess). 
//       It may have from 1 to 5 elements, in this order: 
//       [xc0 yc0 r0] or [xc0 yc0 a0 b0] or [xc0 yc0 a0 b0 g0]
//       The number of expected output parameters chooses the fitting type.
//
//  HISTORY:
//  1.0 / 2011-10-17: first release
//  2.0 / 2015-05-12: 
//    - Tilted ellipses can now be fitted.
//    - The type of fitting is now set by the number of expected output parameters.
//    - Automatic first guess improved a lot.
//    - Constrains on parameters added (automatic guess).
//    - Demo improved and updated. Help updated.

    if argn(2)==0	// => help
        head_comments ellipfit
        Xc=0, Yc=0, a=0, b=0, g=0
        return
        
    elseif argn(2)>1  // Fitting
        Xp = Xp(:).'
        Yp = Yp(:).'
        Xc = mean(Xp)
        Yc = mean(Yp)
        a = strange(Xp)*0.7
        b = strange(Yp)*0.7
        if argn(1)<4
            minR  = min([a b])/2
            a = max([a b])/2
            maxR  = 10*a
            minXc = Xc-maxR
            maxXc = Xc+maxR
            minYc = Yc-maxR
            maxYc = Yc+maxR
            Pmin = [minXc minYc minR]'
            Pmax = [maxXc maxYc maxR]'
            g = 0
        else
            // We estimate the orientation
            [u,tmp] = reglin(Xp,Yp)
            g = atand(u)
            // We rotate data in order to estimate true a and b
            C = cosd(-g)
            S = sind(-g)
            Xpr = C*Xp-S*Yp
            Ypr = S*Xp+C*Yp
            a = strange(Xpr)*0.6
            b = strange(Ypr)*0.6
            mina  = a/5
            maxa  = a*3
            minb  = b/5
            maxb  = a*3
            minXc = Xc-a*3
            maxXc = Xc+a*3
            minYc = Yc-b*3
            maxYc = Yc+b*3
            
            dg = (1-sqrt(abs(correl(Xp,Yp))))*100
            //mprintf("g=%d dg=%d\n",g,dg)
//            Pmin = [minXc minYc mina minb (g-dg)/180*%pi]'
//            Pmax = [maxXc maxYc maxa maxb (g+dg)/180*%pi]'
            Pmin = [minXc minYc mina minb (g-50)/180*%pi]'
            Pmax = [maxXc maxYc maxa maxb (g+50)/180*%pi]'
        end
        
        if argn(2)==2 then
            Po = [Xc; Yc; a; b ; g]
        else
            Po = Po(:)
            select length(Po)
            case 1, Po = [ Po;   Yc; a ; b ; g ]
            case 2, Po = [ Po(1:2) ; a ; b ; g]
            case 3, Po = [ Po(1:3) ; b ; g ]
            case 4, Po = [ Po(1:4) ; g ]
            end
        end
        
        if argn(1)==3
            [P,res] = datafit(ellipsecarts,[ Xp ; Yp ],"b",Pmin(1:3),Pmax(1:3), Po(1:3));
            Xc = P(1)
            Yc = P(2)
            a = P(3);
        elseif argn(1)==4
            [P,res] = datafit(ellipsecarts,[ Xp ; Yp ],"b",Pmin(1:4),Pmax(1:4), Po(1:4));
            Xc = P(1)
            Yc = P(2)
            a = P(3)
            b = P(4)
            g = 0
        elseif argn(1)==5
            Po(5) = Po(5)/180*%pi;
            [P,res] = datafit(ellipsecarts,[ Xp ; Yp ],"b",Pmin,Pmax, Po(1:5));
            Xc = P(1)
            Yc = P(2)
            a = P(3)
            b = P(4)
            g = P(5)/%pi*180
            if g>90
                g = g-180
            end
            if g<0
                g = g+90
                tmp = a
                a = b
                b = tmp
            end
            
        else
            error("ellipfit: Bad number of output parameters")
        end
    // ======  D E M O  ======================================
    else     // ellipfit demo => demo
        mprintf("ellipfit        // to see the help\nedit ellipfit   // to see the script\n\n")
        // 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)
            g = grand(1,1,"unf",0, 90)
            titcon = "xc= %0.2f \t yc= %0.2f \t a= %0.2f \t b= %0.2f \t g=%d\n"
            mprintf(msprintf("Model: "+titcon, xc, yc, a, b, g)+"\n")
            tit = "Fit / model(w/o noise) : C=[%.2f|%.2f, %.2f|%.2f]  a= %.2f|%.2f  b= %.2f|%.2f  g= %d|%d°"
        else 
            b = a
            g = 0
            titcon = "xc= %0.2f \t yc= %0.2f \t R= %0.2f\n"
            mprintf(msprintf("Model: "+titcon, xc, yc, a)+"\n")
            tit = "Fit|model(w/o noise) : C=[%.2f|%.2f, %.2f|%.2f]  R = %.2f|%.2f"
        end
        an0 = grand(1,1,"unf",-3,3);
        angles = linspace(an0,an0+grand(1,1,"unf",1.5,4),30);
        C = cosd(g)
        S = sind(g)
        x = a*cos(angles);
        y = b*sin(angles);
        xp = C*x - S*y + xc
        yp = S*x + C*y + yc

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

        // Fitting parameters:
        if Xp~="circle"
            [xcF, ycF, aF, bF, gF] = ellipfit(xp,yp)
        else
            [xcF, ycF, aF] = ellipfit(xp,yp)
            bF = aF
            gF = 0
        end
        // Generating the fitting curve
        if geomean([aF bF])<7*geomean([stdev(xp) stdev(yp)])
            t = linspace(0,360,200)
        else
            xpc = xp - xcF
            ypc = yp - ycF
            C = cosd(gF)
            S = sind(gF)
            x = xpc*C + ypc*S
            y = -xpc*S + ypc*C
            tmp = x/aF
            k = find(tmp<-1)
            tmp(k) = -1
            k = find(tmp>1)
            tmp(k) = 1
            t = acosd(tmp)     //  [0 180]
            tmp = y/aF
            k = find(tmp<-1)
            tmp(k) = -1
            k = find(tmp>1)
            tmp(k) = 1
            t2 = asind(tmp)    //  [-90 90]
            k = find(t2<0)
            t(k) = -t(k) 
            k = find(t<0)
            t(k) = t(k)+360
            t = linspace(min(t), max(t), 200)
        end
        x = aF*cosd(t);
        y = bF*sind(t);
        C = cosd(gF);
        S = sind(gF);
        xF = (C*x - S*y) + xcF;
        yF = (S*x + C*y) + ycF;
        
        // Plotting
        clf
        drawlater
        plot(xp,yp,"b.","markSize",3)
        ax = gca(); 
        ax.isoview = "on";
        db = ax.data_bounds;
        plot(xF, yF, "r")
        ax.data_bounds = db;
        if Xp~="circle"
            xtitle(msprintf(tit,xcF,xc,ycF,yc,aF,a,bF,b,g,gF))
            mprintf(msprintf("Fit  : "+titcon, xcF, ycF, aF, bF, gF)+"\n")
        else
            xtitle(msprintf(tit,xcF,xc,ycF,yc,aF,a))
            mprintf(msprintf("Fit  : "+titcon, xcF, ycF, aF)+"\n")
        end
        drawnow
        Xc=0, Yc=0, a=0, b=0, g=0
    end
endfunction

// ----------- U T I L I T Y  for  ellipfit()  ---------------

function e = ellipsecarts(P,Z)
    // P = [ Xc Yc R ]       or 
    // P = [ Xc Yc a b ]     or
    // P = [ Xc Yc a b g ]
    a = P(3)
    b = a
    g = 0
    if length(P)>3, b = P(4); end
    if length(P)>4 then
        g = P(5)    // angle in rad
    end
    C = cos(g)
    S = sin(g)
    xp = Z(1,:)-P(1)
    yp = Z(2,:)-P(2)
    e = sum(((xp*C+yp*S)/a).^2 + ((xp*S-yp*C)/b).^2 - 1)
endfunction
