{-# LANGUAGE RecordWildCards #-}
{-# OPTIONS_GHC -fno-warn-name-shadowing #-}
--------------------------------------------------------------------------------
-- |
-- Module      :  Algorithms.Geometry.PolygonTriangulation.EarClip
-- Copyright   :  (C) David Himmelstrup
-- License     :  see the LICENSE file
-- Maintainer  :  David Himmelstrup
--
-- Ear clipping triangulation algorithms. The baseline algorithm runs in \( O(n^2) \)
-- but has a low constant factor overhead. The z-order hashed variant runs in
-- \( O(n \log n) \).
--
-- References:
--
--  1. https://en.wikipedia.org/wiki/Polygon_triangulation#Ear_clipping_method
--  2. https://en.wikipedia.org/wiki/Z-order_curve
--
--------------------------------------------------------------------------------
module Algorithms.Geometry.PolygonTriangulation.EarClip
  ( earClip
  , earClipRandom
  , earClipHashed
  , earClipRandomHashed
  , zHash
  , zUnHash
  ) where

import           Control.Lens                 ((^.))
import           Control.Monad.Identity
import           Control.Monad.ST             (ST, runST)
import           Control.Monad.ST.Unsafe      (unsafeInterleaveST)
import           Data.Bits
import           Data.Ext
import           Data.Geometry.Boundary       (PointLocationResult (Outside))
import           Data.Geometry.Point          (Point (Point2), ccw', pattern CCW)
import           Data.Geometry.Polygon
import           Data.Geometry.Box
import           Data.Geometry.Triangle       (Triangle (Triangle), inTriangleRelaxed)
import           Data.STRef
import           Data.Vector                  (Vector)
import qualified Data.Vector                  as V
import qualified Data.Vector.Algorithms.Intro as Algo
import qualified Data.Vector.Circular         as CV
import qualified Data.Vector.NonEmpty         as NE
import qualified Data.Vector.Unboxed          as U
import qualified Data.Vector.Unboxed.Mutable  as MU
import           GHC.Exts                     (build)
import           Linear.V2
import           System.Random                (mkStdGen, randomR)

{-
  We can check if a vertex is an ear in O(n) time. Checking all vertices will definitely
  yield at least one ear in O(n^2) time. So, finding N ears will take O(n^3) if done naively.

  Keeping a separate list of possible ears will improve matters. For each possible ear,
  we check if the vertex really is an ear or not. If it isn't, it is deleted from the
  list of possible ears. If it /is/ an ear, the vertex is cut and the neighbours are
  added back to the list of possible ears (if they aren't in the list already).

  So, start with a list of N possible ears, and we might add two vertices to the list
  ever time we find an ear. Since there are only N ears to be found, only 2*N vertices
  can be added to the list of possible ears in the worst case scenario. The list is
  therefore bounded to 3*N and finding all ears is therefore O(n^2).

  Note: When checking if a vertex is an ear, it is sufficient to check against
        reflex vertices. Some implementations keep a separate list of reflex
        vertices for this reason but it does increase the constant factor
        overhead. I think it's better to keep the constant factor low for small values
        of N and use the hashed algorithm for larger values of N.
-}
-- | \( O(n^2) \)
--
--   Returns triangular faces using absolute polygon point indices.
earClip :: (Num r, Ord r) => SimplePolygon p r -> [(Int,Int,Int)]
earClip poly = build gen
  where
    vs = NE.toVector $ CV.vector $ poly^.outerBoundaryVector
    gen :: ((Int,Int,Int) -> b -> b) -> b -> b
    gen cons nil = runST $ do
      vertices <- mutListFromVector vs
      possibleEars <- mutListClone vertices
      let worker len focus = do
            prev <- mutListPrev vertices focus
            next <- mutListNext vertices focus
            if len == 3
              then
                return $ cons (prev, focus, next) nil
              else do
                prevEar <- mutListPrev possibleEars focus
                nextEar <- mutListNext possibleEars focus
                isEar <- earCheck vertices prev focus next
                if isEar
                  then do
                    mutListDelete possibleEars prevEar nextEar
                    mutListDelete vertices prev next -- remove ear

                    case (prevEar /= prev, nextEar /= next) of
                      (True, True)  -> do
                        mutListInsert possibleEars prevEar nextEar prev
                        mutListInsert possibleEars prev nextEar next
                      (True, False) -> do
                        mutListInsert possibleEars prevEar nextEar prev
                      (False, True) -> do
                        mutListInsert possibleEars prevEar nextEar next
                      (False, False) -> return ()

                    cons (prev, focus, next)
                      <$> unsafeInterleaveST (worker (len-1) nextEar)
                  else do -- not an ear
                    mutListDelete possibleEars prevEar nextEar -- remove vertex
                    worker len nextEar
      worker (V.length vs) 0

-- | \( O(n^2) \)
--
--   Returns triangular faces using absolute polygon point indices.
earClipRandom :: (Num r, Ord r) => SimplePolygon p r -> [(Int,Int,Int)]
earClipRandom poly = build gen
  where
    vs = NE.toVector $ CV.vector $ poly^.outerBoundaryVector
    gen :: ((Int,Int,Int) -> b -> b) -> b -> b
    gen cons nil = runST $ do
      vertices <- mutListFromVector vs
      possibleEars <- mutListClone vertices
      shuffled <- newShuffled (V.length vs)
      let worker len = do
            focus <- popShuffled shuffled
            prev <- mutListPrev vertices focus
            next <- mutListNext vertices focus
            if len == 3
              then
                return $ cons (prev, focus, next) nil
              else do
                prevEar <- mutListPrev possibleEars focus
                nextEar <- mutListNext possibleEars focus
                isEar <- earCheck vertices prev focus next
                if isEar
                  then do
                    mutListDelete possibleEars prevEar nextEar
                    mutListDelete vertices prev next -- remove ear

                    case (prevEar /= prev, nextEar /= next) of
                      (True, True)  -> do
                        pushShuffled shuffled prev
                        pushShuffled shuffled next
                        mutListInsert possibleEars prevEar nextEar prev
                        mutListInsert possibleEars prev nextEar next
                      (True, False) -> do
                        pushShuffled shuffled prev
                        mutListInsert possibleEars prevEar nextEar prev
                      (False, True) -> do
                        pushShuffled shuffled next
                        mutListInsert possibleEars prevEar nextEar next
                      (False, False) -> return ()

                    cons (prev, focus, next)
                      <$> unsafeInterleaveST (worker (len-1))
                  else do -- not an ear
                    mutListDelete possibleEars prevEar nextEar -- remove vertex
                    worker len
      worker (V.length vs)

-- | \( O(n \log n) \) expected time.
--
--   Returns triangular faces using absolute polygon point indices.
earClipHashed :: Real r => SimplePolygon p r -> [(Int,Int,Int)]
earClipHashed poly = build gen
  where
    vs = NE.toVector $ CV.vector $ poly^.outerBoundaryVector
    n = V.length vs
    hasher = zHashGen vs
    zHashVec = U.generate n $ \i -> hasher (V.unsafeIndex vs i ^. core)
    gen :: ((Int,Int,Int) -> b -> b) -> b -> b
    gen cons nil = runST $ do
      vertices <- mutListFromVector vs
      zHashes <- mutListSort zHashVec
      possibleEars <- mutListClone vertices
      let worker len focus = do
            prev <- mutListPrev vertices focus
            next <- mutListNext vertices focus
            if len == 3
              then
                return $ cons (prev, focus, next) nil
              else do
                prevEar <- mutListPrev possibleEars focus
                nextEar <- mutListNext possibleEars focus
                isEar <- earCheckHashed hasher vertices zHashes prev focus next
                if isEar
                  then do
                    mutListDelete possibleEars prevEar nextEar
                    mutListDelete vertices prev next -- remove ear
                    mutListDeleteFocus zHashes focus

                    case (prevEar /= prev, nextEar /= next) of
                      (True, True)  -> do
                        mutListInsert possibleEars prevEar nextEar prev
                        mutListInsert possibleEars prev nextEar next
                      (True, False) -> do
                        mutListInsert possibleEars prevEar nextEar prev
                      (False, True) -> do
                        mutListInsert possibleEars prevEar nextEar next
                      (False, False) -> return ()

                    cons (prev, focus, next)
                      <$> unsafeInterleaveST (worker (len-1) nextEar)
                  else do -- not an ear
                    mutListDelete possibleEars prevEar nextEar -- remove vertex
                    worker len nextEar
      worker n 0

-- | \( O(n \log n) \) expected time.
--
--   Returns triangular faces using absolute polygon point indices.
earClipRandomHashed :: Real r => SimplePolygon p r -> [(Int,Int,Int)]
earClipRandomHashed poly = build gen
  where
    vs = NE.toVector $ CV.vector $ poly^.outerBoundaryVector
    n = V.length vs
    hasher = zHashGen vs
    zHashVec = U.generate n $ \i -> hasher (V.unsafeIndex vs i ^. core)
    gen :: ((Int,Int,Int) -> b -> b) -> b -> b
    gen cons nil = runST $ do
      vertices <- mutListFromVector vs
      zHashes <- mutListSort zHashVec
      possibleEars <- mutListClone vertices
      shuffled <- newShuffled (V.length vs)
      let worker len = do
            focus <- popShuffled shuffled
            prev <- mutListPrev vertices focus
            next <- mutListNext vertices focus
            if len == 3
              then
                return $ cons (prev, focus, next) nil
              else do
                prevEar <- mutListPrev possibleEars focus
                nextEar <- mutListNext possibleEars focus
                isEar <- earCheckHashed hasher vertices zHashes prev focus next
                if isEar
                  then do
                    mutListDelete possibleEars prevEar nextEar
                    mutListDelete vertices prev next -- remove ear
                    mutListDeleteFocus zHashes focus

                    case (prevEar /= prev, nextEar /= next) of
                      (True, True)  -> do
                        pushShuffled shuffled prev
                        pushShuffled shuffled next
                        mutListInsert possibleEars prevEar nextEar prev
                        mutListInsert possibleEars prev nextEar next
                      (True, False) -> do
                        pushShuffled shuffled prev
                        mutListInsert possibleEars prevEar nextEar prev
                      (False, True) -> do
                        pushShuffled shuffled next
                        mutListInsert possibleEars prevEar nextEar next
                      (False, False) -> return ()

                    cons (prev, focus, next)
                      <$> unsafeInterleaveST (worker (len-1))
                  else do -- not an ear
                    mutListDelete possibleEars prevEar nextEar -- remove vertex
                    worker len
      worker n

-------------------------------------------------------------------------------
-- Bounding box

-- Returns (minX, widthX, minY, heightY)
zHashGen :: Real r => V.Vector (Point 2 r :+ p) -> (Point 2 r -> Word)
zHashGen v = zHashPoint bounds
  where
    bounds = (minX, realToFrac (maxX-minX), minY, realToFrac (maxY-minY))
    bb = V.foldl1' (<>) $ V.map boundingBox v
    Point2 minX minY = minPoint bb ^. core
    Point2 maxX maxY = minPoint bb ^. core

-------------------------------------------------------------------------------
-- Z-Order
-- https://en.wikipedia.org/wiki/Z-order_curve

zHashPoint :: Real r => (r,Double,r,Double) -> Point 2 r -> Word
zHashPoint (minX, widthX, minY, heightY) (Point2 x y) =
    zHash (V2 x' y')
  where
    x' = round (realToFrac (x-minX) / widthX * zHashMax)
    y' = round (realToFrac (y-minY) / heightY * zHashMax)

zHashMax :: Double
zHashMax = realToFrac zHashMaxW

zHashMaxW :: Word
zHashMaxW = if finiteBitSize zHashMaxW == 32 then 0xFFFF else 0xFFFFFFFF

-- | O(1) Z-Order hash the first half-world of each coordinate.
zHash :: V2 Word -> Word
zHash (V2 a b) = zHashSingle a .|. (unsafeShiftL (zHashSingle b) 1)

-- | O(1) Reverse z-order hash.
zUnHash :: Word -> V2 Word
zUnHash z =
  V2 (zUnHashSingle z) (zUnHashSingle (unsafeShiftR z 1))

zHashSingle :: Word -> Word
zHashSingle w
  | finiteBitSize w == 32 = zHashSingle32 w
  | otherwise             = zHashSingle64 w

zUnHashSingle :: Word -> Word
zUnHashSingle w
  | finiteBitSize w == 32 = zUnHashSingle32 w
  | otherwise             = zUnHashSingle64 w

zHashSingle32 :: Word -> Word
zHashSingle32 w = runIdentity $ do
    w <- pure $ w .&. 0x0000FFFF
    w <- pure $ (w .|. unsafeShiftL w 8)  .&. 0x00FF00FF
    w <- pure $ (w .|. unsafeShiftL w 4)  .&. 0x0F0F0F0F
    w <- pure $ (w .|. unsafeShiftL w 2)  .&. 0x33333333
    w <- pure $ (w .|. unsafeShiftL w 1)  .&. 0x55555555
    pure w

zUnHashSingle32 :: Word -> Word
zUnHashSingle32 w = runIdentity $ do
    w <- pure $ w .&. 0x55555555
    w <- pure $ (w .|. unsafeShiftR w 1)  .&. 0x33333333
    w <- pure $ (w .|. unsafeShiftR w 2)  .&. 0x0F0F0F0F
    w <- pure $ (w .|. unsafeShiftR w 4)  .&. 0x00FF00FF
    w <- pure $ (w .|. unsafeShiftR w 8)  .&. 0x0000FFFF
    pure w

zHashSingle64 :: Word -> Word
zHashSingle64 w = runIdentity $ do
    w <- pure $ w .&. 0x00000000FFFFFFFF
    w <- pure $ (w .|. unsafeShiftL w 16) .&. 0x0000FFFF0000FFFF
    w <- pure $ (w .|. unsafeShiftL w 8)  .&. 0x00FF00FF00FF00FF
    w <- pure $ (w .|. unsafeShiftL w 4)  .&. 0x0F0F0F0F0F0F0F0F
    w <- pure $ (w .|. unsafeShiftL w 2)  .&. 0x3333333333333333
    w <- pure $ (w .|. unsafeShiftL w 1)  .&. 0x5555555555555555
    pure w

zUnHashSingle64 :: Word -> Word
zUnHashSingle64 w = runIdentity $ do
    w <- pure $ w .&. 0x5555555555555555
    w <- pure $ (w .|. unsafeShiftR w 1) .&. 0x3333333333333333
    w <- pure $ (w .|. unsafeShiftR w 2)  .&. 0x0F0F0F0F0F0F0F0F
    w <- pure $ (w .|. unsafeShiftR w 4)  .&. 0x00FF00FF00FF00FF
    w <- pure $ (w .|. unsafeShiftR w 8)  .&. 0x0000FFFF0000FFFF
    w <- pure $ (w .|. unsafeShiftR w 16)  .&. 0x00000000FFFFFFFF
    pure w

-------------------------------------------------------------------------------
-- Shuffled

data Shuffled s = Shuffled
  { shuffleCount  :: STRef s Int
  , shuffleVector :: MU.MVector s Int }

newShuffled :: Int -> ST s (Shuffled s)
newShuffled len = Shuffled <$> newSTRef len <*> U.unsafeThaw (U.enumFromN 0 len)

popShuffled :: Shuffled s -> ST s Int
popShuffled Shuffled{..} = do
  count <- readSTRef shuffleCount
  writeSTRef shuffleCount (count-1)
  let idx = fst $ randomR (0, count-1) (mkStdGen count)
  val <- MU.unsafeRead shuffleVector idx
  MU.unsafeWrite shuffleVector idx =<< MU.unsafeRead shuffleVector (count-1)
  pure val

pushShuffled :: Shuffled s -> Int -> ST s ()
pushShuffled (Shuffled ref vector) val = do
  count <- readSTRef ref
  writeSTRef ref (count+1)
  MU.unsafeWrite vector count val

-------------------------------------------------------------------------------
-- MutList

data MutList s a = MutList
  { mutListIndex   :: (Int -> a)
  , mutListNextVec :: MU.MVector s Int
  , mutListPrevVec :: MU.MVector s Int
  }

-- O(n)
mutListFromVector :: Vector a -> ST s (MutList s a)
mutListFromVector vec = MutList (V.unsafeIndex vec)
  <$> do
    arr <- U.unsafeThaw (U.enumFromN 1 (V.length vec))
    MU.unsafeWrite arr (V.length vec-1) 0
    pure arr
  <*> do
    arr <- U.unsafeThaw (U.enumFromN (-1) (V.length vec))
    MU.unsafeWrite arr 0 (V.length vec-1)
    pure arr

mutListClone :: MutList s a -> ST s (MutList s a)
mutListClone (MutList vec nextVec prevVec) = MutList vec
  <$> MU.clone nextVec
  <*> MU.clone prevVec

mutListNext :: MutList s a -> Int -> ST s Int
mutListNext m idx = MU.unsafeRead (mutListNextVec m) idx

mutListPrev :: MutList s a -> Int -> ST s Int
mutListPrev m idx = MU.unsafeRead (mutListPrevVec m) idx

mutListDelete :: MutList s a -> Int -> Int -> ST s ()
mutListDelete m prev next = do
  MU.unsafeWrite (mutListNextVec m) prev next
  MU.unsafeWrite (mutListPrevVec m) next prev

mutListDeleteFocus :: MutList s a -> Int -> ST s ()
mutListDeleteFocus m focus = do
  prev <- mutListPrev m focus
  next <- mutListNext m focus
  unless (prev == -1) $
    MU.unsafeWrite (mutListNextVec m) prev next
  unless (next == -1) $
    MU.unsafeWrite (mutListPrevVec m) next prev

mutListInsert :: MutList s a -> Int -> Int -> Int -> ST s ()
mutListInsert m before after elt = do
  MU.unsafeWrite (mutListNextVec m) before elt  -- before.next = elt
  MU.unsafeWrite (mutListNextVec m) elt after   -- elt.next = after
  MU.unsafeWrite (mutListPrevVec m) after elt   -- after.prev = elt
  MU.unsafeWrite (mutListPrevVec m) elt before  -- elt.prev = before

mutListSort :: (Ord a, MU.Unbox a) => U.Vector a -> ST s (MutList s a)
mutListSort vec = do
    sorted <- do
      arr <- U.unsafeThaw $ (U.enumFromN 0 n :: U.Vector Int)
      Algo.sortBy (\a b -> compare (U.unsafeIndex vec a) (U.unsafeIndex vec b)) arr
      U.unsafeFreeze arr

    next <- MU.new n
    prev <- MU.new n
    MU.write next
      (U.unsafeIndex sorted (n-1))
      (-1)
    forM_ [0..n-2] $ \i -> do
      MU.write next
        (U.unsafeIndex sorted i)
        (U.unsafeIndex sorted (i+1))
    MU.write prev
      (U.unsafeIndex sorted 0)
      (-1)
    forM_ [1..n-1] $ \i -> do
      MU.write prev
        (U.unsafeIndex sorted i)
        (U.unsafeIndex sorted (i-1))
    pure $ MutList (U.unsafeIndex vec) next prev
  where
    n = U.length vec

-------------------------------------------------------------------------------
-- Ear checking

-- O(n)
earCheck :: (Num r, Ord r) => MutList s (Point 2 r :+ p) -> Int -> Int -> Int -> ST s Bool
earCheck vertices a b c = do
  let pointA = mutListIndex vertices a
      pointB = mutListIndex vertices b
      pointC = mutListIndex vertices c
      trig = Triangle pointA pointB pointC

  let loop elt | elt == a = pure True
      loop elt = do
        let point = mutListIndex vertices elt ^. core
        case inTriangleRelaxed point trig of
          Outside -> loop =<< mutListNext vertices elt
          _       -> pure False
  if ccw' pointA pointB pointC == CCW
    then loop =<< mutListNext vertices c
    else pure False

-- showBinary :: (Integral a, Show a) => a -> String
-- showBinary i = showIntAtBase 2 intToDigit i ""

earCheckHashed :: Real r => (Point 2 r -> Word) -> MutList s (Point 2 r :+ p) -> MutList s Word -> Int -> Int -> Int -> ST s Bool
earCheckHashed hasher vertices zHashes a b c = do
  let pointA = mutListIndex vertices a
      pointB = mutListIndex vertices b
      pointC = mutListIndex vertices c
      trig = Triangle pointA pointB pointC
      trigBB = boundingBox trig
      lowPt = minPoint trigBB ^. core
      highPt = maxPoint trigBB ^. core
      -- (lowPt, highPt) = triangleBoundingBox trig

      minZ = hasher lowPt
      maxZ = hasher highPt

  let upwards up
        | up == -1 || upZ > maxZ = pure True
        | inTriangleRelaxed pointUp trig /= Outside = pure False
        | otherwise = upwards =<< mutListNext zHashes up
        where
          upZ = mutListIndex zHashes up
          pointUp = mutListIndex vertices up ^. core
      downwards down
        | down == -1 || downZ < minZ = pure True
        | inTriangleRelaxed pointDown trig /= Outside = pure False
        | otherwise = downwards =<< mutListPrev zHashes down
        where
          downZ = mutListIndex zHashes down
          pointDown = mutListIndex vertices down ^. core
      bidirectional up down
        | up == -1   || upZ > maxZ   = downwards down
        | down == -1 || downZ < minZ = upwards up
        | up /= a && up /= b && inTriangleRelaxed pointUp trig /= Outside = pure False
        | down /= a && down /= b && inTriangleRelaxed pointDown trig /= Outside = pure False
        | otherwise = do
          up' <- mutListNext zHashes up
          down' <- mutListPrev zHashes down
          bidirectional up' down'
        where
          upZ = mutListIndex zHashes up
          downZ = mutListIndex zHashes down
          pointUp = mutListIndex vertices up ^. core
          pointDown = mutListIndex vertices down ^. core
  if ccw' pointA pointB pointC == CCW
    then bidirectional b b
    else pure False