```module Reanimate.Morph.LineBend
( lineBend
, lineBendRaw
) where

import qualified Data.Vector            as V
import           Linear.Matrix          (det22)
import           Linear.Metric
import           Linear.V2
import           Linear.Vector
import           Reanimate.Math.Common
import           Reanimate.Math.Polygon
import           Reanimate.Morph.Common

lineBend :: Trajectory
lineBend = lineBend' True

lineBendRaw :: Trajectory
lineBendRaw = lineBend' False

-- Note: Returns polygon with n+1 elements.
lineBend' :: Bool -> Trajectory
lineBend' corrected (a,b) = \t ->
let u = 1 - t
-- phi = V.zipWith (\l r -> u*l + t*r) phi_a phi_b
phi = V.zipWith (\l r -> lerpAngle l r t) phi_a phi_b
-- alpha_zero = u * alpha_a_zero + t * alpha_b_zero
alpha_zero = lerpAngle alpha_a_zero alpha_b_zero t
alpha = V.scanl (+) alpha_zero phi
bigE =
V.sum \$ V.zipWith (\diff alp -> squared diff * squared (cos alp)) lengths_diff alpha
bigF =
V.sum \$ V.zipWith (\diff alp -> squared diff * sin alp * cos alp) lengths_diff alpha
bigG =
V.sum \$ V.zipWith (\diff alp -> squared diff * squared (sin alp)) lengths_diff alpha
bigU =
2 * V.sum
(V.zipWith3 (\len_a len_b alp -> (u*len_a + t*len_b) * cos alp) lengths_a lengths_b alpha)
bigV =
2 * V.sum
(V.zipWith3 (\len_a len_b alp -> (u*len_a + t*len_b) * sin alp) lengths_a lengths_b alpha)
lam1 = det22 (V2 (V2 bigU bigF) (V2 bigV bigG)) /
det22 (V2 (V2 bigE bigF) (V2 bigF bigG))
lam2 = det22 (V2 (V2 bigE bigU) (V2 bigF bigV)) /
det22 (V2 (V2 bigE bigF) (V2 bigF bigG))
s_vect = V.fromList
[ -0.5 * squared (lengths_diff V.! n) *
(lam1 * cos (alpha V.! n) +
lam2 * sin (alpha V.! n))
| n <- [0 .. pSize a-1]]
lengths = V.zipWith3 (\l r s -> u*l + t*r + if corrected then s else 0) lengths_a lengths_b s_vect
movingCenter :: V2 Double
movingCenter = lerp t (realToFrac <\$> pCentroid b) (realToFrac <\$> pCentroid a)
centerAng :: Double
centerAng = lerpAngle centoid_angle_a centoid_angle_b t
centerLen = u*centoid_len_a + t*centoid_len_b
-- V2 x_zero y_zero = lerp t (realToFrac <\$> b V.! 0) (realToFrac <\$> a V.! 0)
V2 x_zero y_zero = movingCenter + V2 (cos centerAng * centerLen) (sin centerAng * centerLen)
x_modifiers = V.zipWith (*) lengths (V.map cos alpha)
y_modifiers = V.zipWith (*) lengths (V.map sin alpha)
xs = V.scanl (+) x_zero x_modifiers
ys = V.scanl (+) y_zero y_modifiers
in mkPolygon \$ V.map (fmap realToFrac) \$ V.zipWith V2 xs ys
where
squared x = x*x
centoid_angle_a :: Double
centoid_angle_a = lineAngle (pCentroid a + V2 1 0) (pCentroid a) (pAccess a 0)
centoid_angle_b = lineAngle (pCentroid b + V2 1 0) (pCentroid b) (pAccess b 0)
centoid_len_a :: Double
centoid_len_a = realToFrac \$ approxDist (pCentroid a) (pAccess a 0)
centoid_len_b = realToFrac \$ approxDist (pCentroid b) (pAccess b 0)
alpha_a_zero = lineAngle (pAccess a 0 + V2 1 0) (pAccess a 0) (pAccess a 1)
alpha_b_zero = lineAngle (pAccess b 0 + V2 1 0) (pAccess b 0) (pAccess b 1)
lengths_a = computeLength a
lengths_b = computeLength b
lengths_diff' = V.zipWith (\l r -> abs (l-r)) lengths_a lengths_b
lengths_tol = 0.0001 * V.maximum lengths_diff'
lengths_diff = V.map (max lengths_tol) lengths_diff'
phi_a = computePhi a
phi_b = computePhi b

computeLength :: Polygon -> V.Vector Double
computeLength poly = V.fromList
[ realToFrac \$ approxDist this next
| n <- [0 .. pSize poly-1]
, let this = pAccess poly n
next = pAccess poly (pNext a n)
]
computePhi poly = V.fromList \$
[ negate \$ angle' (next-this) ((prev-this) ^* (-1))
| n <- [1 .. pSize poly-1]
, let prev = pAccess poly (n-1)
this = pAccess poly n
next = pAccess poly (pNext a n)
]

lerpAngle :: Double -> Double -> (Double -> Double)
lerpAngle fromAng toAng t
| abs (fromAng - (toAng+2*pi)) < abs (fromAng - toAng) = (1-t)*fromAng + t*(toAng+2*pi)
| abs (fromAng - (toAng-2*pi)) < abs (fromAng - toAng) = (1-t)*fromAng + t*(toAng-2*pi)
| otherwise = (1-t)*fromAng + t*toAng

-- Angle from a through b to c. Measured on the right-hand side, from 0 to tau
lineAngle :: V2 Rational -> V2 Rational -> V2 Rational -> Double
lineAngle a b c = angle' (a-b) (c-b)

angle' :: V2 Rational -> V2 Rational -> Double
angle' a b = atan2 (realToFrac \$ crossZ a b) (realToFrac \$ dot a b)
```