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)