{-# LANGUAGE BangPatterns #-}
module Bezier where

import Data.List
import Data.Ord

import qualified Data.Vector.Generic as G
import Numeric.LinearAlgebra

type Plane = Vector Float

ĵ :: Vector Float
ĵ = fromList [0,1,0]

type BufferID = Int

data Polygonization = Polygonization
    { curveBufferID     :: BufferID
    , curveStartIndex   :: Int
    , curveSegmentCount :: Int
    }
 deriving Show

type AllocatedRange = Polygonization

data Curve = Curve
    { curvePlane    :: Maybe Plane  -- ^ Curve is embedded in this plane.
    , curveA        :: Vector Float -- ^ 3d start point.
    , curveB        :: Vector Float -- ^ 2nd 3d control point.
    , curveC        :: Vector Float -- ^ 3rd 3d control point.
    , curveD        :: Vector Float -- ^ 3d stop point.
    , curveSegments :: Maybe Polygonization
    }

-- Although this function accepts an index, 'subdivideCurve' increases the
-- index by one after each call, so it is safe to ignore the index passed.
type StorePoint m = BufferID -> Int -> Vector Float {- 3d -} -> m ()

xz :: Vector Float -> (Float,Float)
xz v = (v!0, v!2)

subdivideCurve :: Monad m => Float -> Curve -> AllocatedRange -> StorePoint m -> m Polygonization
subdivideCurve δ curve range store | curveSegmentCount range < 2 = return range { curveSegmentCount = 0 }
subdivideCurve δ curve range store = do
    let ts = sort $ take (curveSegmentCount range - 2) $ sampleCurve δ curve
    n <- (\f -> foldr f return (zip [0..] (0 : ts)) 0) $ \(i,t) ret _ -> do
        let v = bezierEval curve t
        store (curveBufferID range) (curveStartIndex range + i) v
        ret i
    let v = bezierEval curve 1.0
    store (curveBufferID range) (curveStartIndex range + n + 1) v
    return range { curveSegmentCount = n + 2 }

-- | Return an unsorted list of curve parameter values to use as polygonization
-- vertices.  The first argument is a desired maximum error distance to the
-- true curve.
--
-- Note: Caller will likely want to sort and to augment the result with 0 and 1
-- to sample the curve's end points.
--
-- The technique used here is from "Sampling Planar Curves Using
-- Curvature-Based Shape Analysis" by Tatiana and Vitaly Surazhsky.  In a
-- nutshell: we divide the half circle into n equal parts and sample the curve
-- whenever the tangent line is at one of these angles to a fixed axis.
sampleCurve :: Real a => a -> Curve -> [Float]
sampleCurve δ curve
    | Just plane <- curvePlane curve
    , G.init plane == ĵ  = sampleCurveXZ δ curve
    | otherwise          = sampleCurveTwist δ curve

-- | Bounding box diagonal length for a cubic curve's 4 control points.
curveBBDiag :: Fractional b => Curve -> b
curveBBDiag Curve{curveA = a, curveB = b, curveC = c, curveD = d} =
    realToFrac $ norm_2 ( mx - mn )
 where
    mx = maximum [a,b,c,d]
    mn = minimum [a,b,c,d]


-- | The Surazhsky curve sampling where the Y coordinates are ignored in order
-- that we can treat the curve in only two dimensions.
sampleCurveXZ :: Real a => a -> Curve -> [Float]
sampleCurveXZ δ curve@Curve{curveA = a, curveB = b, curveC = c, curveD = d}  = do
    let (x0,y0) = xz a -- We assume the curve is embedded in the xz plane so that
        (x1,y1) = xz b -- we can use a technique for 2 dimensional curves.
        (x2,y2) = xz c -- TODO: Use the correct projection here for curves
        (x3,y3) = xz d -- embedded in other planes.
        -- This computation of n was not detailed in the paper, but I figure it
        -- ought to be alright when δ is small relative to the curve's bounding
        -- box.  It was derived using the aproximation tan θ ≃ θ for small θ.
        n = ceiling (pi/sqrt(2*realToFrac δ/dist))
            where dist = curveBBDiag curve
        tx = x3 - x0
        ty = y3 - y0
        θ₀ = atan2 ty tx -- For our fixed axis, we use the line connecting the
                         -- curve's end points.  This has the nice property
                         -- that the first two returned sample points will be
                         -- at the maximum distance the curve makes with its
                         -- straight line aproximation.
                         --
                         -- Idea: It occurs to me that we can extend the
                         -- Surazhsky method to 3 dimensions by obtaining a
                         -- parameterization using polar coordinates about this
                         -- axis.  We might ignore curvature about this axis as
                         -- less important.
    θ <- [ θ₀ + pi/fromIntegral n*fromIntegral i | i <- [ 0 .. n - 1 ] ]
    tangents (cos θ) (sin θ) x0 y0 x1 y1 x2 y2 x3 y3

twistAbout :: Vector Float -> Vector Float -> Vector Float -> (Float,Float)
twistAbout ĥ k̂ v = let { x = dot ĥ v
                       ; y = v - scale x ĥ
                       }
                 in (x, (if dot y k̂ <0 then negate else id) $ realToFrac $ norm_2 y)


-- | The Surazhsky curve sampling where the middle two control points are
-- rotated about the line connecting the curve's end points in order that they
-- become coplanar so that we can treat the curve in only two dimensions.
sampleCurveTwist :: Real a => a -> Curve -> [Float]
sampleCurveTwist δ curve@Curve{curveA = a, curveB = b, curveC = c, curveD = d}  = do
    let (x0,y0) = twistAbout ĥ k̂ a
        (x1,y1) = twistAbout ĥ k̂ b
        (x2,y2) = twistAbout ĥ k̂ c
        (x3,y3) = twistAbout ĥ k̂ d
        -- ĥ = normalize $ vector $ map realToFrac $ toList (d - a)
        ĥ = let v = d - a in scale (1 / realToFrac (norm_2 v)) v
        k̂ = let { k = minimumBy (comparing $ \x -> abs (dot x ĥ)) $ map (subtract a) [b,c]
                ; v = k - scale (dot k ĥ) ĥ }
            in scale (1 / realToFrac (norm_2 v)) v
        -- This computation of n was not detailed in the paper, but I figure it
        -- ought to be alright when δ is small relative to the curve's bounding
        -- box.  It was derived using the aproximation tan θ ≃ θ for small θ.
        n = 1 + ceiling (pi/sqrt(realToFrac δ/dist))
            where dist = curveBBDiag curve
        tx = x3 - x0
        ty = y3 - y0
        θ₀ = atan2 ty tx -- For our fixed axis, we use the line connecting the
                         -- curve's end points.  This has the nice property
                         -- that the first two returned sample points will be
                         -- at the maximum distance the curve makes with its
                         -- straight line aproximation.
                         --
                         -- Idea: It occurs to me that we can extend the
                         -- Surazhsky method to 3 dimensions by obtaining a
                         -- parameterization using polar coordinates about this
                         -- axis.  We might ignore curvature about this axis as
                         -- less important.
    θ <- [ θ₀ + fromIntegral i * pi/fromIntegral n | i <- [ 0 .. n - 1 ] ]
    tangents (cos θ) (sin θ) x0 y0 x1 y1 x2 y2 x3 y3


tangents :: (Ord a, Floating a) =>
                  a -> a -> a -> a -> a -> a -> a -> a -> a -> a -> [a]
tangents tx ty x0 y0 x1 y1 x2 y2 x3 y3 = filter relevent $ quadraticRoot a b c
 where
    relevent x | x < 1e-12     = False
               | 1 - x < 1e-12 = False
               | otherwise     = True
    a = tx*((y3 - y0) + 3*(y1 - y2)) - ty*((x3 - x0) + 3*(x1 - x2))
    b = 2*(tx*((y2 + y0) - 2*y1) - ty*((x2 + x0) - 2*x1))
    c = tx*(y1 - y0) - ty*(x1 - x0)

-- | @quadraticRoot a b c@ find the real roots of the quadratic equation
-- @a x^2 + b x + c = 0@.  It will return one, two or zero roots.
quadraticRoot :: (Ord a, Floating a) => a -> a -> a -> [a]
quadraticRoot a b c
  | a == 0 && b == 0 = []
  | a == 0 = [-c/b]
  | otherwise = let d = b*b - 4*a*c
                    q | b < 0     = - (b - sqrt d) / 2
                      | otherwise = - (b + sqrt d) / 2
                    x1 = q/a
                    x2 = c/q
                in case compare d 0 of
                    LT -> []
                    EQ -> [x1]
                    GT -> [x1,x2]


bezierEval :: Curve -> Float -> Vector Float
bezierEval Curve{curveA=a,curveB=b,curveC=c,curveD=d} t =
    fromList [ bernsteinEval x t, bernsteinEval y t, bernsteinEval z t]
 where
    poly i = fromList [a!i,b!i,c!i,d!i]
    x = poly 0
    y = poly 1
    z = poly 2

-- | Evaluate the bernstein polynomial using the horner rule adapted
-- for bernstein polynomials.
bernsteinEval :: (G.Vector v a, Fractional a) => v a -> a -> a
bernsteinEval v _
  | G.length v == 0 = 0
bernsteinEval v _
  | G.length v == 1 = G.unsafeHead v
bernsteinEval v t =
  go t (fromIntegral n) (G.unsafeIndex v 0 * u) 1
  where u = 1-t
        n = fromIntegral $ G.length v - 1
        go !tn !bc !tmp !i
          | i == n = tmp + tn*G.unsafeIndex v n
          | otherwise =
            go (tn*t) -- tn
            (bc*fromIntegral (n-i)/(fromIntegral i + 1)) -- bc
            ((tmp + tn*bc*G.unsafeIndex v i)*u) -- tmp
            (i+1) -- i