{-# OPTIONS_GHC -Wno-unrecognised-pragmas #-}
{-# HLINT ignore "Redundant bracket" #-}
module Approx.BiArc (
    bezier2biarcs
) where

import qualified Graphics.CubicBezier as B
import qualified Graphics.BiArc as BA
import qualified Graphics.CircularArc as CA
import qualified Graphics.Line as L
import Graphics.Curve
import Graphics.Path
import Graphics.Point
import Utils

import Data.Bool (bool)
import Control.Lens
import Linear

eps :: Double
eps = 0.0001
maxiter :: Double
maxiter = 10

-- Approximate a bezier curve with biarcs (Left) and line segments (Right)
-- M.A. Sabin, The use of piecewise forms for the numerical representation of shape (1977)
bezier2biarcs :: B.CubicBezier
              -> Double
              -> [PathCommand]
bezier2biarcs mbezier resolution
    -- Degenerate curve: all points on the same line -> it is a line 
    | L.isOnLine (L.fromPoints (B._p2 mbezier) (B._p1 mbezier)) (B._c1 mbezier) &&
      L.isOnLine (L.fromPoints (B._p2 mbezier) (B._p1 mbezier)) (B._c2 mbezier)
        = [LineTo (toPoint (B._p2 mbezier))]
    -- Degenerate curve: p1 == c1, don't split
    | B._p1 mbezier == B._c1 mbezier
        = approxSpiral mbezier
    -- Degenerate curve: p2 == c2, don't split
    | B._p2 mbezier == B._c2 mbezier
        = approxSpiral mbezier
    -- Split by the inflexion points (if any)
    | otherwise
        = byInflection (B.inflectionPoints mbezier)
    where
        order a b | b < a = (b, a)
                  | otherwise = (a, b)

        byInflection [t] = approxSpiral b1 ++ approxSpiral b2
            where
                (b1, b2) = B.splitAt mbezier t

        byInflection [t1, t2] = approxSpiral b1 ++ approxSpiral b2 ++ approxSpiral b3
            where
                (it1, it2') = order t1 t2

                -- Make the first split and save the first new curve. The second one has to be splitted again
                -- at the recalculated t2 (it is on a new curve)                
                it2 = (1 - it1) * it2'

                (b1, toSplit) = B.splitAt mbezier it1
                (b2, b3) = B.splitAt toSplit it2

        byInflection _ = approxSpiral mbezier

        -- Apprixmate one bezier spiral
        approxSpiral :: B.CubicBezier -> [PathCommand]
        approxSpiral bezier
            -- Approximate bezier length. if max length is smaller than resolution, do not approximate
            | B.maxArcLength bezier < resolution
                = [LineTo (toPoint (B._p2 bezier))]
            -- Edge case: start- and endpoints are the same
            | B._p1 bezier == B._p2 bezier
                = splitAndRecur 0.5
            -- Edge case: control lines are parallel
            | L._m t1 == L._m t2 || isNaN (L._m t1) && isNaN (L._m t2)
                = splitAndRecur 0.5
            -- Biarc triangle has the wrong orientation
            -- Curve looks like this: https://pomax.github.io/bezierinfo/images/chapters/decasteljau/df92f529841f39decf9ad62b0967855a.png
            | B.isClockwise bezier /= isClockwise3 (B._p1 bezier) (B._p2 bezier) v
                = splitAndRecur 0.5
            -- Unstable approximation: split the bezier into half, it will switch to linear approximation if the segments get too small
            | not (isStable biarc)
                = splitAndRecur 0.5
            -- Approximation is not close enough yet, refine
            | maxDistance > resolution
                = splitAndRecur maxDistanceAt
            -- Desired case: approximation is stable and close enough
            | otherwise
                = biarc2path biarc

            where
                -- Edge case: P1==C1 or P2==C2
                -- there is no derivative at P1 or P2, use the other control point
                c1 = bool (B._c1 bezier) (B._c2 bezier) (B._p1 bezier == B._c1 bezier)
                c2 = bool (B._c2 bezier) (B._c1 bezier) (B._p2 bezier == B._c2 bezier)

                -- V: Intersection point of tangent lines
                t1 = L.fromPoints (B._p1 bezier) c1
                t2 = L.fromPoints (B._p2 bezier) c2
                v = L.intersection t1 t2

                -- G: incenter point of the triangle (P1, V, P2)
                dP2V = distance (B._p2 bezier) v
                dP1V = distance (B._p1 bezier) v
                dP1P2 = distance (B._p1 bezier) (B._p2 bezier)
                g = (dP2V *^ B._p1 bezier + dP1V *^ B._p2 bezier + dP1P2 *^ v) ^/ (dP2V + dP1V + dP1P2)

                -- Calculate the BiArc
                biarc = BA.fromPoints (B._p1 bezier) (B._p1 bezier - c1) (B._p2 bezier) (B._p2 bezier - c2) g

                (maxDistanceAt, maxDistance) = calculateMaxDistance bezier biarc

                splitAndRecur t = let (b1, b2) = B.splitAt bezier t
                                   in approxSpiral b1 ++ approxSpiral b2

biarc2path :: BA.BiArc -> [PathCommand]
biarc2path biarc = map
    (\arc -> ArcTo (toPoint (CA._c arc)) (toPoint (CA._p2 arc)) (CA.isClockwise arc))
    [BA._a1 biarc, BA._a2 biarc]

-- Heuristics for unstable biarc: the radius of at least one of the arcs 
-- is too big or too small. Not too scientific...
isStable :: BA.BiArc -> Bool
isStable biarc
    = not (CA._r (BA._a1 biarc) > 99999 || CA._r (BA._a1 biarc) < 0.001 ||
           CA._r (BA._a2 biarc) > 99999 || CA._r (BA._a2 biarc) < 0.001)

-- Calculate the maximum approximation error along the radial direction
-- D.J. Walton, D.S. Meek, Approximation of a planar cubic Bezier spiral by circular arcs (1996)
calculateMaxDistance :: B.CubicBezier -> BA.BiArc -> (Double, Double)
calculateMaxDistance bezier biarc
    -- This should not happenm but if, split the bezier at the middle
    | tj == -1 = (0.5, 0x7FEFFFFFFFFFFFFF)
    | otherwise = bigger (bigger (tj, dj) (t0, d0)) (t1, d1)
    where
        tj = findRadialIntersection bezier biarc (BA.jointAt biarc)
        dj = distance (pointAt bezier tj) (pointAt biarc (BA.jointAt biarc))

        g arc u = dot (pointAt bezier u - CA._c arc) (B.firstDerivativeAt bezier u)
        g' arc u = quadrance (B.firstDerivativeAt bezier u) +
                   dot (pointAt bezier u - CA._c arc) (B.secondDerivativeAt bezier u)

        bigger f@(_, df) s@(ts, ds)
            | ts == -1 = f
            | df > ds = f
            | otherwise = s

        -- Valid in (0,tj]
        t0 = findRoot (g (BA._a1 biarc)) (g' (BA._a1 biarc)) eps tj
        d0 = abs ((distance (pointAt bezier t0) (CA._c (BA._a1 biarc))) - (CA._r (BA._a1 biarc)))
        -- Valid in [tj,1)
        t1 = findRoot (g (BA._a2 biarc)) (g' (BA._a2 biarc)) tj (1 - eps)
        d1 = abs ((distance (pointAt bezier t1) (CA._c (BA._a2 biarc))) - (CA._r (BA._a2 biarc)))

-- Takes a paramater `t` fore the `biarc` and calculates the related parameter fo
-- the `bezier` (which is the intersection point in the radial direction)
findRadialIntersection :: B.CubicBezier -> BA.BiArc -> Double -> Double
findRadialIntersection bezier biarc t
    | t == 0 || t == 1 = t
    | otherwise = findRoot (\u -> dot (pointAt bezier u - p) h) (\u -> dot (B.firstDerivativeAt bezier u) h) 0 1
    where
        p = pointAt biarc t
        c = CA._c $ if' (t <= BA.jointAt biarc) (BA._a1 biarc) (BA._a2 biarc)
        m = p - c
        h = normalize $ V2 (negate (m ^. _y)) (m ^. _x)

-- Tries to find the root of f in interval [lowerBound,upperBound] using a combination of
-- Newton and bisection methods.
-- It is supposed to have at most one solution. If no solution is found, returns -1
findRoot :: (Double -> Double) -> (Double -> Double) -> Double -> Double -> Double
findRoot f df lowerBound upperBound
    | fl * fu > 0 = -1
    | fl == 0 = lowerBound
    | fu == 0 = upperBound
    | otherwise = iter maxiter fl fu lowerBound upperBound ((lowerBound + upperBound) / 2)
    where
        fl = f lowerBound
        fu = f upperBound

        iter i fmin fmax lb ub root
            -- we're good, or if i==0, we may not reached tolarence yet, but hopefully it is close enough
            | abs fx < eps || i <= 0 = root
            -- overshoot or undershoot -> switch to bisection
            | n < lb || n > ub
                = if' (fmin * fx < 0)
                    (iter (i-1) fmin fx lb root ((lb + root) / 2))
                    (iter (i-1) fx fmax root ub ((root + ub) / 2))
            -- Newton step
            | otherwise
                = iter (i-1) fmin fmax lb ub n
            where
                fx = f root
                h = fx / df root
                n = root - h