/** This contains functions to fit 5 points to a conic section (either an
    ellipse, parabola, or in infintely precise cases, a hyperbola or circle.

    See:
    https://www.johndcook.com/blog/2023/06/19/conic-through-five-points/
*/

use Matrix.frink

/** Fit the following 5 points to a conic section.  This returns the
    coefficients [B,C,D,E,F] for the equation

    A x^2 + B y^2 + C x y + D x + E y === 0  and A is always chosen to be 1.
*/ 
fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   A = new Matrix[[[y1^2, x1 y1, x1, y1, 1],
                   [y2^2, x2 y2, x2, y2, 1],
                   [y3^2, x3 y3, x3, y3, 1],
                   [y4^2, x4 y4, x4, y4, 1],
                   [y5^2, x5 y5, x5, y5, 1]]]

   B = new Matrix[[-x1^2, -x2^2, -x3^2, -x4^2, -x5^2]].transpose[]
   return A.solve[B]
}

/** This fits 5 points to a conic section and returns a text version of the
    defining equation.   This is suitable for passing to Frink's interval
    graphing routines such as Plot3D.frink
*/
fitPointsToText[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   [B,C,D,E,F] = map["inputForm", fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]]
   return "x^2 + $B y^2 + $C x y + $D x + $E y + $F = 0"
}

/** This fits 5 points to a conic section and returns an array of functions
    where each function is a function of x that returns the y value for that
    value of x.

    THINK ABOUT: These might be undefined where B is zero.  How to handle that?
*/
fitPointsToYFunctions[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   [b,c,d,e,f] = points = fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   [B,C,D,E,F] = map["inputForm", points]
   A = 1

   if (b==0)
      return parseToExpression["[{|x| -(x ($A x + $D) + $F) / ($C x + $E)}]"]
   
   n1 = "sqrt[($C x + $E)^2 - 4 $B (x ($A x + $D) + $F)]"
   n2 = "$C x + $E"
   
   y1 = "-($n1 + $n2)/(2 $B)"
   y2 = "-(-$n1 + $n2)/(2 $B)"
   // println["$y1"]
   // println["$y2"]
   f1 = parseToExpression["{|x| $y1}"]
   f2 = parseToExpression["{|x| $y2}"]
   return [f1, f2]
}


/** This fits 5 points to a conic section and returns an array of functions
    where each function is a function of y that returns the x value for that
    value of y.

    THINK ABOUT: These might be undefined where B is zero.  How to handle that?
*/
fitPointsToXFunctions[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   [b,c,d,e,f] = points = fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   [B,C,D,E,F] = map["inputForm", points]
   A = 1

   if (a==0)
      return parseToExpression["[{|y| -(y ($B y + $E) + $F) / ($C y + $D)}]"]
   
   n1 = "sqrt[($C y + $D)^2 - 4 $A (y ($B y + $E) + $F)]"
   n2 = "$C y + $D"
   
   x1 = "-($n1 + $n2)/(2 $A)"
   x2 = "-(-$n1 + $n2)/(2 $A)"
   // println["$y1"]
   // println["$y2"]
   f1 = parseToExpression["{|y| $x1}"]
   f2 = parseToExpression["{|y| $x2}"]
   return [f1, f2]
}


/** Fits 5 points to a conic section and returns a string indicating if they
    comprise an ellipse, hyperbola, or parabola. */
characterizePoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   [B,C,D,E,F] = fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   A = 1
   det = 4 A B - C^2
   //println["det is $det, A=$A, B=$B, C=$C"]
   if det > 0
      if B == 0 and A == C
         return "circle"
      else
         return "ellipse"
   if det < 0
      return "hyperbola"
   if det == 0
      return "parabola"
}

/** Fits 5 points to a conic section and returns a string indicating if they
    comprise an ellipse, hyperbola, or parabola. */
plotPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   str = fitPointsToText[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   xmin = min[[x1,x2,x3,x4,x5]]
   xmax = max[[x1,x2,x3,x4,x5]]
   ymin = min[[y1,y2,y3,y4,y5]]
   ymax = max[[y1,y2,y3,y4,y5]]
   // println[str]
   unsafeEval["use Plot2D.frink"]
   p = new Plot2D[]
   p.setDoublings[12]
   p.setBounds[xmin*1.25,xmax*1.25,ymin*1.25,ymax*1.25]
   g = p.plot[str]
   g.color[0,0,1,.5]
   w = (xmax-xmin) / 50
   g.fillEllipseCenter[x1, -y1, w, w]
   g.fillEllipseCenter[x2, -y2, w, w]
   g.fillEllipseCenter[x3, -y3, w, w]
   g.fillEllipseCenter[x4, -y4, w, w]
   g.fillEllipseCenter[x5, -y5, w, w]
   return g
}

/*
// Sample usage
plotPoints[8,8, 8,4, -2,4, 3,-1, 5,8].show[]
*/