{-# LANGUAGE AllowAmbiguousTypes   #-}
{-# LANGUAGE DeriveAnyClass        #-}
{-# LANGUAGE DeriveGeneric         #-}
{-# LANGUAGE DeriveTraversable     #-}
{-# LANGUAGE DerivingVia           #-}
{-# LANGUAGE FlexibleInstances     #-}
{-# LANGUAGE MultiParamTypeClasses #-}
{-# LANGUAGE RecordWildCards       #-}
{-# LANGUAGE ScopedTypeVariables   #-}
{-# LANGUAGE StandaloneDeriving    #-}
{-# LANGUAGE TypeApplications      #-}
{-# LANGUAGE UndecidableInstances  #-}

module Math.Bezier.Cubic
  ( Bezier(..)
  , bezier, bezier', bezier''
  , curvature, squaredCurvature
  , subdivide
  , ddist, closestPoint
  )
  where

-- base
import Data.List.NonEmpty
  ( NonEmpty(..) )
import Data.Monoid
  ( Ap(..) )
import Data.Semigroup
  ( ArgMin, Min(..), Arg(..) )
import GHC.Generics
  ( Generic, Generic1 )

-- acts
import Data.Act
  ( Act(..)
  , Torsor
    ( (-->) )
  )

-- deepseq
import Control.DeepSeq
  ( NFData, NFData1 )

-- generic-data
import Generic.Data
  ( GenericProduct(..), Generically1(..) )

-- groups
import Data.Group
  ( Group )

-- groups-generic
import Data.Group.Generics
  ()

-- MetaBrush
import qualified Math.Bezier.Quadratic as Quadratic
  ( Bezier(Bezier), bezier )
import Math.Epsilon
  ( epsilon )
import Math.Module
  ( Module (..)
  , lerp
  , Inner(..), squaredNorm
  )
import Math.Roots
  ( realRoots )

--------------------------------------------------------------------------------

-- | Points defining a cubic Bézier curve (Bernstein form).
--
-- @ p0 @ and @ p3 @ are endpoints, whereas @ p1 @ and @ p2 @ are control points.
data Bezier p
  = Bezier
    { p0, p1, p2, p3 :: !p }
  deriving stock ( Show, Generic, Generic1, Functor, Foldable, Traversable )
  deriving ( Semigroup, Monoid, Group )
    via GenericProduct ( Bezier p )
  deriving Applicative
    via Generically1 Bezier
  deriving anyclass ( NFData, NFData1 )

deriving via Ap Bezier p
  instance Act v p => Act v ( Bezier p )

-- | Cubic Bézier curve.
bezier :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> p
bezier ( Bezier {..} ) t =
  lerp @v t
    ( Quadratic.bezier @v ( Quadratic.Bezier p0 p1 p2 ) t )
    ( Quadratic.bezier @v ( Quadratic.Bezier p1 p2 p3 ) t )


-- | Derivative of cubic Bézier curve.
bezier' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> v
bezier' ( Bezier {..} ) t
  = ( 3 *^ )
  $ lerp @v t
      ( lerp @v t ( p0 --> p1 ) ( p1 --> p2 ) )
      ( lerp @v t ( p1 --> p2 ) ( p2 --> p3 ) )

-- | Second derivative of a cubic Bézier curve.
bezier'' :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> v
bezier'' ( Bezier {..} ) t
  = ( 6 *^ )
  $ lerp @v t
      ( p1 --> p0 ^+^ p1 --> p2 )
      ( p2 --> p1 ^+^ p2 --> p3 )

-- | Curvature of a quadratic Bézier curve.
curvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r
curvature bez t = sqrt $ squaredCurvature @v bez t

-- | Square of curvature of a quadratic Bézier curve.
squaredCurvature :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> r -> r
squaredCurvature bez t
  | sq_nm_g' < epsilon
  = 1 / 0
  | otherwise
  = ( sq_nm_g' * squaredNorm @v g'' - ( g' ^.^ g'' ) ^ ( 2 :: Int ) )
  / ( sq_nm_g' ^ ( 3 :: Int ) )
  where
    g', g'' :: v
    g'  = bezier'  @v bez t
    g'' = bezier'' @v bez t
    sq_nm_g' :: r
    sq_nm_g' = squaredNorm @v g'


-- | Subdivide a cubic Bézier curve into two parts.
subdivide :: forall v r p. ( Torsor v p, Module r v ) => Bezier p -> r -> ( Bezier p, Bezier p )
subdivide ( Bezier {..} ) t = ( Bezier p0 q1 q2 pt, Bezier pt r1 r2 p3 )
  where
    pt, s, q1, q2, r1, r2 :: p
    q1 = lerp @v t p0 p1
    s  = lerp @v t p1 p2
    r2 = lerp @v t p2 p3
    q2 = lerp @v t q1 s
    r1 = lerp @v t s  r2
    pt = lerp @v t q2 r1

-- | Polynomial coefficients of the derivative of the distance to a cubic Bézier curve.
ddist :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> [ r ]
ddist ( Bezier {..} ) c = [ a0, a1, a2, a3, a4, a5 ]
  where
    v, v', v'', v''' :: v
    v    = c  --> p0
    v'   = p0 --> p1
    v''  = p1 --> p0 ^+^ p1 --> p2
    v''' = p0 --> p3 ^+^ 3 *^ ( p2 --> p1 )

    a0, a1, a2, a3, a4, a5 :: r
    a0 = v ^.^ v'
    a1 = 3 * squaredNorm v' + 2 * v ^.^ v''
    a2 = 9 * v' ^.^ v'' + v ^.^ v'''
    a3 = 6 * squaredNorm v'' + 4 * v' ^.^ v'''
    a4 = 5 * v'' ^.^ v'''
    a5 = squaredNorm v'''

-- | Finds the closest point to a given point on a cubic Bézier curve.
closestPoint :: forall v r p. ( Torsor v p, Inner r v, RealFloat r ) => Bezier p -> p -> ArgMin r ( r, p )
closestPoint pts@( Bezier {..} ) c = pickClosest ( 0 :| 1 : roots ) -- todo: also include the self-intersection point if one exists
  where
    roots :: [ r ]
    roots = filter ( \ r -> r > 0 && r < 1 ) ( realRoots $ ddist @v pts c )

    pickClosest :: NonEmpty r -> ArgMin r ( r, p )
    pickClosest ( s :| ss ) = go s q nm0 ss
      where
        q :: p
        q  = bezier @v pts s
        nm0 :: r
        nm0 = squaredNorm ( c --> q :: v )
        go t p nm []  = Min ( Arg nm ( t, p ) )
        go t p nm ( t' : ts )
          | nm' < nm  = go t' p' nm' ts
          | otherwise = go t  p  nm  ts
          where
            p' :: p
            p' = bezier @v pts t'
            nm' :: r
            nm' = squaredNorm ( c --> p' :: v )