module Dodge.Creature.Boid where

import Data.Maybe
import Linear
import Dodge.Creature.Radius
import Control.Lens
--import Control.Monad.Reader
import Dodge.Base
import Dodge.Data.World
import Geometry
import qualified IntMapHelp as IM

interpWith :: Float -> Point2 -> Point2 -> Point2
interpWith x a b = x *.* a +.+ (1 - x) *.* b

invertEncircleDistP :: Float -> Creature -> Point2 -> Creature -> Point2
invertEncircleDistP d tcr cenp cr =
    ypos
        +.+ d *.* reflectIn (cenp -.- ypos) (squashNormalizeV (cpos -.- cenp))
  where
    cpos = cr  ^. crPos . _xy
    ypos = tcr ^. crPos . _xy

encircleDistP :: Float -> Creature -> Point2 -> Creature -> Point2
encircleDistP d tcr cenp cr = ypos +.+ d *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    ypos = tcr ^. crPos . _xy

encircleP :: Creature -> Point2 -> Creature -> Point2
encircleP tcr cenp cr = ypos +.+ 50 *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    ypos = tcr ^. crPos . _xy

--f x = 150 * sigmoid (x-10)

encircleCloseP :: Creature -> Point2 -> Creature -> Point2
encircleCloseP tcr cenp cr = ypos +.+ f (max 0 (magV (ypos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    ypos = tcr ^. crPos . _xy
    f x = 150 * sigmoid (x -10)

forbidFlee ::
    (Creature -> Point2 -> Creature -> Point2) ->
    Creature ->
    Point2 ->
    Creature ->
    Point2
forbidFlee f tcr cenp cr
    | ptargTest = tpos
    | otherwise = ptarg
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    ptarg = f tcr cenp cr
    ptargTest = isLHS cpos (cpos +.+ rotateV (negate (pi / 2)) (cpos -.- tpos)) ptarg

--      && isRHS cpos (cpos +.+ rotateV (pi/3) (cpos -.- tpos)) ptarg
--targBehindCrit = isLHS cpos (vNormal $ cpos +.+ unitVectorAtAngle (_crDir cr)) ptarg

pincerP :: Creature -> Point2 -> Creature -> Point2
pincerP tcr cenp cr = tpos +.+ splitp -- +.+ 25 *.* (normalizeV $ tpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    splitp
        | isLHS cenp cpos tpos =
            150 *.* orthCenpTpos
        | otherwise =
            negate 150 *.* orthCenpTpos
    --d = min 150 (dist cpos tpos)
    --orthCenpTpos = safeNormalizeV (vNormal $ tpos -.- cpos)
    orthCenpTpos = squashNormalizeV (vNormal $ tpos -.- cenp)

pincerP''' :: Creature -> Point2 -> Creature -> Point2
pincerP''' tcr cenp cr = interpWith (sigmoid $ 0.05 * dtcen) cenawayp cenclosep
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    dtcen = dist tpos cenp
    f x = 150 * sigmoid (x -10)
    cenawayp
        | dist cenp tpos < dist cpos tpos = tpos +.+ splitp
        | otherwise = cenp +.+ splitp
    splitp
        | isLHS cenp cpos tpos =
            f (max 0 (magV (tpos -.- cenp) - 80)) *.* orthCenpTpos
        | otherwise =
            negate (f $ max 0 $ magV (tpos -.- cenp) - 80) *.* orthCenpTpos
    orthCenpTpos = squashNormalizeV (vNormal $ tpos -.- cenp)
    cenclosep =
        tpos +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)

pincerP' :: Creature -> Point2 -> Creature -> Point2
pincerP' tcr cenp cr
    | dist cenp tpos > dist cpos tpos =
        cenp +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)
    | otherwise = tpos +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    f x = 150 * sigmoid (x -10)

pincerP'' :: Creature -> Point2 -> Creature -> Point2
pincerP'' tcr cenp cr
    | dist cenp tpos > dist cpos tpos && isLHS cenp cpos tpos =
        cenp +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (vNormal $ tpos -.- cenp)
    | dist cenp tpos > dist cpos tpos =
        cenp +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (vNormal $ cenp -.- tpos)
    | otherwise = tpos +.+ f (max 0 (magV (tpos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    f x = 150 * sigmoid (x -10)

encircle :: Creature -> IM.IntMap Creature -> Creature -> Point2
encircle tcr crs cr
    | length crs <= 1 = ypos
    | otherwise = ypos +.+ f (max 0 (magV (ypos -.- cenp) - 80)) *.* squashNormalizeV (cpos -.- cenp)
  where
    cpos = cr  ^. crPos . _xy
    ypos = tcr ^. crPos . _xy
    f x = 150 * sigmoid (x -10)
    cenp = centroid (map (^. crPos . _xy) $ IM.elems crs)

lineOrth :: Creature -> IM.IntMap Creature -> Creature -> Point2
lineOrth tcr crs cr = p
  where
    ypos = tcr ^. crPos . _xy
    cpos = cr  ^. crPos . _xy
    ps = map (^. crPos . _xy) $ IM.elems crs
    cen = (1 / fromIntegral (length ps)) *.* foldr1 (+.+) ps
    p
        | dist cen ypos < 20 = ypos
        | otherwise = errorClosestPointOnLine 500 ypos (ypos +.+ vNormal (cen -.- ypos)) cpos

holdForm :: Creature -> IM.IntMap Creature -> Creature -> Point2
holdForm ycr crs cr = p
  where
    ypos = ycr ^. crPos . _xy
    cpos = cr  ^. crPos . _xy
    ps = map (^. crPos . _xy) $ IM.elems crs
    cen = centroid ps
    p
        | dist cen ypos < 20 = ypos
        | otherwise = ypos +.+ cpos -.- cen

lineUp :: Creature -> IM.IntMap Creature -> Creature -> Point2
lineUp ycr crs cr = p
  where
    ypos = ycr ^. crPos . _xy
    cpos = cr  ^. crPos . _xy
    ps = map (^. crPos . _xy) $ IM.elems crs
    cen = centroid ps
    p = (0.05 *.* ypos) +.+ (0.95 *.* errorClosestPointOnLine 500 cen ypos cpos)

-- not nice, a kind of encircle
spreadOut :: Creature -> IM.IntMap Creature -> Creature -> Point2
spreadOut ycr crs cr = p
  where
    ypos = ycr ^. crPos . _xy
    cpos = cr  ^. crPos . _xy
    ps = map (^. crPos . _xy) $ IM.elems crs
    cen = centroid ps
    p
        | dist cen ypos < 30 = ypos
        | otherwise = ypos +.+ (spreadFactor *.* cpos -.- cen)
    spreadFactor
        | dist ypos cpos > 90 = 1
        | otherwise = 1.5

swarmUsingCenter ::
    (Creature -> Point2 -> Creature -> Creature) ->
    (Point2 -> Creature -> Creature) ->
    World ->
    Creature ->
    Creature
swarmUsingCenter updT upd w cr = fromMaybe (upd cenp cr) $ do
    i <- _targetCr $ _crIntention cr
    tcr <- w ^? cWorld . lWorld . creatures . ix i
    return $ updT tcr cenp cr
  where
    cid = _crID cr
    cenp = _crGroupCenter $ _creatureGroups (_lWorld (_cWorld w)) IM.! _crGroupID (_crGroup $ _creatures (_lWorld (_cWorld w)) IM.! cid)

flockChaseTarget ::
    -- | Update with target
    (Creature -> IM.IntMap Creature -> Creature -> Creature) ->
    -- | Update without target
    (IM.IntMap Creature -> Creature -> Creature) ->
    World ->
    Creature ->
    Creature
flockChaseTarget updT upd w cr = fromMaybe (upd crs cr) $ do
    i <- _targetCr $ _crIntention cr
    tcr <- w ^? cWorld . lWorld . creatures . ix i
    return $ updT tcr crs cr
  where
    is = _swarm $ _crGroup cr
    crs = IM.restrictKeys (w ^. cWorld . lWorld . creatures) is

flockPointTarget ::
    (Creature -> IM.IntMap Creature -> Creature -> Point2) ->
    -- | Function for determining target
    (Creature -> World -> Maybe Creature) ->
    World ->
    Creature ->
    Creature
flockPointTarget _ _ _ = id
--flockPointTarget ::
--    (Creature -> IM.IntMap Creature -> Creature -> Point2) ->
--    -- | Function for determining target
--    (Creature -> World -> Maybe Creature) ->
--    World ->
--    Creature ->
--    Creature
--flockPointTarget f targFunc w cr = case targFunc cr w of
--    Nothing -> cr
--    Just crTarg -> cr  & crActionPlan . apImpulse .~ mvPointMeleeTarg p cr crTarg
--      where
--        is = _swarm $ _crGroup cr
--        crs = IM.restrictKeys (w ^. cWorld . lWorld . creatures) is
--        p = f crTarg crs cr

--flockToPointUsing ::
--    (Creature -> Point2 -> Creature -> Point2) ->
--    (Point2 -> Creature -> Creature -> [Impulse]) ->
--    Creature ->
--    Reader World Creature
--flockToPointUsing pf mvf cr = reader $ \w -> case _targetCr $ _crIntention cr of
--    Nothing -> cr
--    Just tcr -> cr & crActionPlan . apImpulse .~ mvf ptarg cr tcr
--      where
--        cenp = w ^?! cWorld . lWorld . creatureGroups . ix (cr ^?! crGroup . crGroupID) . crGroupCenter
--           --  crGroupCenter $ _creatureGroups (_cWorld w) IM.! _crGroupID (_crGroup cr)
--        ptarg = pf tcr cenp cr

--flockToPointUsing' ::
--    (Creature -> Point2 -> Creature -> Point2) ->
--    (Point2 -> Creature -> Creature -> [Impulse]) ->
--    World ->
--    Creature ->
--    Creature
--flockToPointUsing' pf mvf w cr = fromMaybe cr $ do
--    i <- _targetCr $ _crIntention cr
--    tcr <- w ^? cWorld . lWorld . creatures . ix i
--    let ptarg = pf tcr cenp cr
--    return $ cr -- & crActionPlan . apImpulse .~ mvf ptarg cr tcr
--      where
--        cenp = w ^?! cWorld . lWorld . creatureGroups . ix (cr ^?! crGroup . crGroupID) . crGroupCenter

--flockFunc ::
--    (Creature -> Point2 -> Creature -> Point2) ->
--    -- | Function for determining target
--    (Creature -> World -> Maybe Creature) ->
--    Creature ->
--    Reader World Creature
--flockFunc f targFunc cr = reader $ \w -> case targFunc cr w of
--    Nothing -> cr
--    Just crTarg -> cr -- & crActionPlan . apImpulse .~ mvPointMeleeTarg p cr crTarg
--      where
--        cenp = w ^?! cWorld . lWorld . creatureGroups . ix (cr ^?! crGroup . crGroupID) . crGroupCenter
--        p = f crTarg cenp cr

--flockCenterFunc ::
--    (Creature -> Point2 -> Creature -> Point2) ->
--    -- | Function for determining target
--    (Creature -> World -> Maybe Creature) ->
--    Creature ->
--    Reader World Creature
--flockCenterFunc f targFunc cr = reader $ \w -> case targFunc cr w of
--    Nothing -> cr
--    Just crTarg -> cr -- & crActionPlan . apImpulse .~ mvPointMeleeTarg p cr crTarg
--      where
--        cenp = w ^?! cWorld . lWorld . creatureGroups . ix (cr ^?! crGroup . crGroupID) . crGroupCenter
--        p = f crTarg cenp cr

--flockPointTargetR ::
--    (Creature -> IM.IntMap Creature -> Creature -> Point2) ->
--    -- | Function for determining target
--    (Creature -> World -> Maybe Creature) ->
--    Creature ->
--    Reader World Creature
--flockPointTargetR f targFunc cr = reader $ \w -> case targFunc cr w of
--    Nothing -> cr
--    Just crTarg -> cr -- & crActionPlan . apImpulse .~ mvPointMeleeTarg p cr crTarg
--      where
--        is = _swarm $ _crGroup cr
--        --crs = IM.restrictKeys (_creatures (_cWorld w)) is
--        crs = IM.restrictKeys (w ^. cWorld . lWorld . creatures) is
--        p = f crTarg crs cr

meleeHeadingMove ::
    -- | max turn speed
    Float ->
    -- | min turn speed
    Float ->
    -- | turn speed cutoff angle
    Float ->
    -- | move speed
    Float ->
    -- | target point
    Point2 ->
    -- | start creature
    Creature ->
    -- | target creature
    Creature ->
    [Impulse]
meleeHeadingMove maxta minta tacutoff speed tp cr tcr
    | dist tpos cpos < combinedRad + 5
        && abs (_crDir cr - argV (tpos -.- cpos)) < tacutoff
        && _meleeCooldown (_crType cr) == 0 =
        [Melee (_crID tcr), Turn pi]
    | dist tpos cpos < combinedRad + 5
        && abs (_crDir cr - argV (tpos -.- cpos)) < tacutoff =
        [TurnToward tpos minta]
    | abs (_crDir cr - argV (tp -.- cpos)) < tacutoff =
        [MoveForward speed, TurnToward tp maxta, RandomTurn maxta]
    | otherwise = [MoveForward speed, TurnToward tp minta, RandomTurn maxta]
  where
    cpos = cr  ^. crPos . _xy
    tpos = tcr ^. crPos . _xy
    combinedRad = crRad (cr ^. crType) + crRad (tcr ^. crType)

mvPointMeleeTarg :: Point2 -> Creature -> Creature -> [Impulse]
mvPointMeleeTarg p cr crT
    | dist tpos cpos < combinedRad + 5
        && abs (_crDir cr - argV (tpos -.- cpos)) < pi / 4
        && _meleeCooldown (_crType cr) == 0 =
        [Melee (_crID crT)]
    | dist tpos cpos < combinedRad + 5
        && abs (_crDir cr - argV (tpos -.- cpos)) < pi / 4 =
        [TurnToward tpos 0.05]
    | abs (_crDir cr - argV (p -.- cpos)) < pi / 4 =
        [MoveForward 3, TurnToward p 0.2, RandomTurn 0.2]
    | otherwise = [MoveForward 3, TurnToward p 0.05, RandomTurn 0.2]
  where
    cpos = cr  ^. crPos . _xy
    tpos = crT ^. crPos . _xy
    combinedRad = crRad (cr ^. crType) + crRad (crT ^. crType)