/** 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/
    But note that that page uses non-standard swapped B and C coefficients
    which have been corrected here.

    Standard form used here is:
    A x^2 + B x y + C y^2 + D x + E y + F === 0

    Here are some interesting notes:

    * If the (B x y) term is nonzero, that means the conic section has been
      rotated and is not axis-aligned.  This file contains functions to
      calculate that rotation and to de-rotate the conic section.  Even if the
      curve has been offset, rotation can be determined from the B coefficient.

    * The (D x) term indicates x offset.  If this term is nonzero, the curve
      has been translated along the x axis.

    * The (E y) term indicates y offset.  If this term is nonzero, the curve
      has been translated along the y axis.

    * An offset conic can be centered by the centerConic function.

    * There are a lot of equivalent equations that represent the same curve.
      These routines try to canonicalize those by keeping the A coefficient
      to be 1.  You can divide the coefficients A through F by A to normalize.
      This can be achieved by tne normalizeConic function.

    * F is a scale parameter.

   Curves can be plotted with the plot... functions which use interval
   arithmetic techniques for easy plotting.

    https://math.stackexchange.com/questions/596016/how-to-convert-the-general-form-of-ellipse-equation-to-the-standard-form

    TODO:  Calculate foci, original vertices and covertices, original foci,
      parametric plot
      x = a cos[theta], y = b sin[theta], circumference, area

    see:
    https://www.wolframalpha.com/input?i=ellipse+A+x%5E2+%2B+C+y%5E2+-+F+%3D+0
*/

use Matrix.frink
use Plot2D.frink
use geometry.frink

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

    A x^2 + B x y + C y^2 + D x + E y + F === 0

    and A is always chosen to be 1.
*/ 
fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5] :=
{
   A = new Matrix[[[x1 y1, y1^2, x1, y1, 1],
                   [x2 y2, y2^2, x2, y2, 1],
                   [x3 y3, y3^2, x3, y3, 1],
                   [x4 y4, y4^2, x4, y4, 1],
                   [x5 y5, y5^2, 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] = fitPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   A = 1
   return coefficientsToText[A, B, C, D, E, F]
}

/** Returns a text version of the defining equation given the coefficients.
    This is suitable for passing to Frink's interval 
    graphing routines such as Plot3D.frink
*/
coefficientsToText[A,B,C,D,E,F] :=
{
   [A,B,C,D,E,F] = map["inputForm", [A, B, C, D, E, F]]
   return "$A x^2 + $B x y + $C y^2 + $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.  This can be used for standard plotting.

    THINK ABOUT: These might be undefined where C 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 (c==0)
      return parseToExpression["[{|x| -(x ($A x + $D) + $F) / ($B x + $E)}]"]
   
   n1 = "sqrt[($B x + $E)^2 - 4 $C (x ($A x + $D) + $F)]"
   n2 = "$B x + $E"
   
   y1 = "-($n1 + $n2)/(2 $C)"
   y2 = "-(-$n1 + $n2)/(2 $C)"

   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 C 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 = A = 1

   // This never happens because a is always 1?
   if (a==0)
      return parseToExpression["[{|y| -(y ($C y + $E) + $F) / ($B y + $D)}]"]
   
   n1 = "sqrt[($B y + $D)^2 - 4 $A (y ($C y + $E) + $F)]"
   n2 = "$B 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, parabola, or circle.  */
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 =  B^2 - 4 A C
   //println["det is $det, A=$A, B=$B, C=$C"]
   if det < 0
      if C == 0 and A == B
         return "circle"
      else
         return "ellipse"
   if det > 0
      return "hyperbola"
   if det == 0
      return "parabola"
}

/** Fits 5 points to a conic section and plots the curve and the control
    points.
*/
plotPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,xmin,xmax,ymin,ymax,drawGrid=false] :=
{
   str = fitPointsToText[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5]
   g = plotEquation[str,xmin,xmax,ymin,ymax,drawGrid]
   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
}

/** Fits 5 points to a conic section and plots the curve and the control
    points. */
plotPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,drawGrid=false] :=
{
   [xmin, xmax] = minmax[[x1,x2,x3,x4,x5]]
   [ymin, ymax] = minmax[[y1,y2,y3,y4,y5]]
   return plotPoints[x1,y1,x2,y2,x3,y3,x4,y4,x5,y5,xmin,xmax,ymin,ymax,drawGrid]
}

/** Given the coefficients of a conic section, plot it. */
plotConic[A,B,C,D,E,F,xmin,xmax,ymin,ymax,drawGrid=false] :=
{
   str = coefficientsToText[A,B,C,D,E,F]
   return plotEquation[str,xmin,xmax,ymin,ymax,drawGrid]
}

/** Given a string indicating an equation, plot it. */
plotEquation[str,xmin,xmax,ymin,ymax,drawGrid=false] :=
{
   p = new Plot2D[]
   p.setDoublings[12]
   p.setBounds[xmin,xmax,ymin,ymax]
   p.setDrawGrid[drawGrid]
   g = p.plot[str]
   return g
}

/** Returns the center of a conic section as [cx, cy].  This is undefined for
    a parabola. */
centerOfConic[A,B,C,D,E,F] :=
{
   // Center of the ellipse
   det = (4 A C - B^2)
   cx = (B E - 2 C D) / det
   cy = (B D - 2 A E) / det
   return [cx, cy]
}

/** Centers a conic section by translating its center and returning new
    coefficients. The D and E coefficients will be zero after this step. */
centerConic[A,B,C,D,E,F] :=
{
   [cx, cy] = centerOfConic[A,B,C,D,E,F]
   [A1,B1,C1,D1,E1,F1] = translateConic[A,B,C,D,E,F,-cx, -cy]

   // Force D1 and E1 (x and y offsets) to be exactly 0
   return[A1,B1,C1,0 D1,0 E1, F1]
}


/**  Find the angle of rotation (counterclockwise).  When the xy term
     (coefficient B) is 0, there is no rotation.  Even if the conic is
     decentered, the rotation can be determined by only considering term B
*/
rotationOfConic[A,B,C,D,E,F] :=
{
   // This follows the convention in
   // https://www.anirdesh.com/math/algebra/rotation-of-conics.php
   // https://www.anirdesh.com/math/algebra/ellipse-rotation.php
   
   if B == 0
      theta = 0 radians
   else
      theta = 1/2 arccot[(A - C)/B]

   return theta
}

/** Rotate a conic counterclockwise by the specified angle and returns the
    new conic coefficients. */
rotateConic[A,B,C,D,E,F,theta] :=
{
   theta = negate[theta]  // The equations below are for clockwise so negate
   // See
   //  https://www.anirdesh.com/math/algebra/rotation-of-conics.php
   
   ct = cos[theta]
   ct2 = ct^2
   c2t = cos[2 theta]
   st = sin[theta]
   st2 = st^2
   s2t = sin[2 theta]
   
   A1 = A ct2 + 1/2 B s2t + C st2
   B1 = (C-A) s2t + B c2t 
   C1 = A st2 -1/2 B s2t + C ct2
   D1 = D ct + E st
   E1 = -D st + E ct
   F1 = F

   return [A1,B1,C1,D1,E1,F1]
}


/** Derotate a conic section and return its new coefficients.  If a conic
    section is derotated, the B coefficient will be zero.
*/
derotateConic[A,B,C,D,E,F] :=
{
   theta = rotationOfConic[A,B,C,D,E,F]
   [A1,B1,C1,D1,E1,F1] = rotateConic[A,B,C,D,E,F,-theta]
   
   // Force B to be exactly 0
   return [A1,0 B1,C1,D1,E1,F1]
}

/** Normalize a conic to have coefficient A = 1.  This does not change the
    underlying curve.
*/
normalizeConic[A,B,C,D,E,F] :=
{
   // This was divide[[A,B,C,D,E,F],A] but that was less precise
   return [1, B/A, C/A, D/A, E/A, F/A]
}


/** This function characterizes an ellipse and returns some of its parameters.

    returns:
    [theta, cx, cy, eccentricity, A1, B1, C1, D1, E1, F1, a, b, fx, fy]
*/
characterizeEllipse[A,B,C,D,E,F]  :=
{
   theta = rotationOfConic[A,B,C,D,E,F]

   [cx, cy] = centerOfConic[A,B,C,D,E,F]

   eccentricity = eccentricity[A,B,C,D,E,F]

   [A1,B1,C1,D1,E1,F1] = translateConic[A,B,C,D,E,F,-cx,-cy]

   [A2,B2,C2,D2,E2,F2] = derotateConic[A1,B1,C1,D1,E1,F1]
   [A3,B3,C3,D3,E3,F3] = normalizeConic[A2,B2,C2,D2,E2,F2]
   a = sqrt[-F3/A3]    // x axis radius, A3 should be 0
   b = sqrt[-F3/C3]    // y axis radius

   // Calculate foci
   if A3 <= C3
   {
      fx1 = sqrt[-F3(1/A3 - 1/C3)]
      fx2 = -fx1
      fy1 = fy2 = 0 C3
   } else
   {
      fy1 = sqrt[-F3(1/C3 - 1/A3)]
      fy2 = -fy1
      fx1 = fx2 = 0 A3
   }
   
   return [theta, cx, cy, eccentricity, A3, B3, C3, D3, E3, F3, a, b, fx1, fy1, fx2, fy2]
}


/** This function characterizes an ellipse given 5 points on the ellipse and
    returns some of its parameters.

    returns:
       [theta, cx, cy, eccentricity, A1, B1, C1, D1, E1, F1, a, b]
*/
characterizeEllipse[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

   return characterizeEllipse[A,B,C,D,E,F]
}


/** Calculate the eccentricity of a conic section. */
eccentricity[A,B,C,D,E,F] :=
{
   // see https://en.wikipedia.org/wiki/Eccentricity_(mathematics)#General_form
   eta = - realSignum[new Matrix[[[A, B/2, D/2], [B/2, C, E/2], [D/2, E/2, F]]].det[]]
   t = sqrt[(A-C)^2 + B^2]
   return  sqrt[(2 t) / (eta (A+C) + t)]
}


/** Calculate the eccentricity of a conic section. */
eccentricity[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

   println["in eccentricity wrapper [$A, $B, $C, $D, $E, $F]"]
   return eccentricity[A,B,C,D,E,F]
}


/** Translates a conic section in standard form by dx, dy

    See:
    https://stemandmusic.in/maths/coordinate-geometry/conics/conicTrans.php
*/
translateConic[A,B,C,D,E,F,dx,dy] :=
{
   // A conic section translated by dx, dy gives:
   // A (x - dx)^2 + B (y - dy) (x - dx) + C(y - dy)^2 + D (x - dx) + E (y - dy) + F 
   A1 = A   
   B1 = B
   C1 = C
// Opposite sign convention (which turns out not to be what we want I think)
//   D1 = 2 A dx + B dy + D
//   E1 = B dx + 2 C dy + E
//   F1 = A dx^2 + B dx dy + C dy^2 + D dx + E dy + F
   D1 = D - 2 A dx - B dy
   E1 = E - B dx - 2 C dy
   F1 = F + A dx^2 + B dx dy + C dy^2 - D dx - E dy
   
   return [A1, B1, C1, D1, E1, F1]
}

/** This uses the geometry.frink library to create a translation that is first
    a translate then a rotate.  It returns the coefficients of an affine
    transform that can be passed to geometry.frink affineTransform[x,y,coeffs]
    or inverseAffineTransform[xp, yp, coeffs]
*/
makeTranslateThenRotate[dx, dy, theta] :=
{
   v1 = makeTranslate[dx, dy]
   v2 = makeRotate[theta]
   v3 = composeTransformation[v1, v2]
   return v3
}

// Calculate the parameters of the original orbit. */
originalParameters[A, B, C, D, E, F] :=
{
   [theta, cx, cy, eccentricity, A1, B1, C1, D1, E1, F1, a, b, fx1, fy1, fx2, fy2] = characterizeEllipse[A,B,C,D,E,F]
   transform = makeTranslateThenRotate[cx, cy, theta]
//   println["Transform is $transform"]

   // Calculate vertices and covertices
   if a >= b
   {
      major = a
      minor = b
      [a1, b1] = affineTransform[ a, 0, transform]
      [a2, b2] = affineTransform[-a, 0, transform]
      [c1, d1] = affineTransform[ 0, b, transform]
      [c2, d2] = affineTransform[ 0,-b, transform]
   } else
   {
      major = b
      minor = a
      [a1, b1] = affineTransform[ 0, b, transform]
      [a2, b2] = affineTransform[ 0,-b, transform]
      [c1, d1] = affineTransform[ a, 0, transform]
      [c2, d2] = affineTransform[-a, 0, transform]
   }
   

   // Calculate foci
   [fx1p, fy1p] = affineTransform[fx1, fy1, transform]
   [fx2p, fy2p] = affineTransform[fx2, fy2, transform]

   return [theta, cx, cy, major, minor, a1, b1, a2, b2, c1, d1, c2, d2, fx1p, fy1p, fx2p, fy2p]
}


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