module Dodge.Creature.Boid
    where
import Dodge.Data
import Dodge.Base
import Geometry
--import Geometry.ConvexPoly

import Control.Monad.Reader
import Control.Lens
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 = _crPos cr
    ypos = _crPos tcr

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

encircleP :: Creature -> Point2 -> Creature -> Point2
encircleP tcr cenp cr = ypos +.+ 50 *.* squashNormalizeV (cpos -.- cenp)
  where 
    cpos = _crPos cr
    ypos = _crPos tcr
    --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 = _crPos cr
    ypos = _crPos tcr
    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 = _crPos cr
    tpos = _crPos tcr
    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 = _crPos cr
    tpos = _crPos tcr
    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 = _crPos cr
    tpos = _crPos tcr
    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 = _crPos cr
    tpos = _crPos tcr
    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 = _crPos cr
    tpos = _crPos tcr
    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 = _crPos cr
    ypos = _crPos tcr
    f x = 150 * sigmoid (x-10)
    cenp = centroid (map _crPos $ IM.elems crs)

lineOrth :: Creature -> IM.IntMap Creature -> Creature -> Point2
lineOrth tcr crs cr = p
  where 
    ypos = _crPos tcr
    cpos = _crPos cr
    ps = map _crPos $ 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 = _crPos ycr
    cpos = _crPos cr
    ps = map _crPos $ IM.elems crs 
    cen = (1 / fromIntegral (length ps)) *.* foldr1 (+.+) 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 = _crPos ycr
    cpos = _crPos cr
    ps = map _crPos $ IM.elems crs 
    cen = (1 / fromIntegral (length ps)) *.* foldr1 (+.+) 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 = _crPos ycr
    cpos = _crPos cr
    ps = map _crPos $ IM.elems crs 
    cen = (1 / fromIntegral (length ps)) *.* foldr1 (+.+) 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 = case _targetCr $ _crIntention cr of
    Nothing -> upd cenp cr
    Just tcr -> updT tcr cenp cr
  where
    cid = _crID cr
    cenp = _crGroupCenter $ _creatureGroups w IM.! _crGroupID (_crGroup $ _creatures w IM.! cid)

flockChaseTarget
    :: (Creature -> IM.IntMap Creature -> Creature -> Creature) -- ^ Update with target
    -> (IM.IntMap Creature -> Creature -> Creature) -- ^ Update without target
    -> World
    -> Creature
    -> Creature
flockChaseTarget updT upd w cr = case _targetCr $ _crIntention cr of
    Nothing -> upd crs cr
    Just tcr -> updT tcr crs cr
  where
    is = _swarm $ _crGroup cr
    crs = IM.restrictKeys (_creatures w) is

flockPointTarget
    :: (Creature -> IM.IntMap Creature -> Creature -> Point2)
    -> (Creature -> World -> Maybe Creature)    -- ^ Function for determining target
    -> 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 (_creatures w) 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 = _crGroupCenter $ _creatureGroups 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 = case _targetCr $ _crIntention cr of
    Nothing -> cr
    Just tcr -> cr & crActionPlan . apImpulse .~ mvf ptarg cr tcr
      where
        cenp = _crGroupCenter $ _creatureGroups w IM.! _crGroupID (_crGroup cr)
        ptarg = pf tcr cenp cr

flockFunc
    :: (Creature -> Point2 -> Creature -> Point2)
    -> (Creature -> World -> Maybe Creature)    -- ^ Function for determining target
    -> 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 = _crGroupCenter $ _creatureGroups w IM.! _crGroupID (_crGroup cr)
        p = f crTarg cenp cr

flockCenterFunc
    :: (Creature -> Point2 -> Creature -> Point2)
    -> (Creature -> World -> Maybe Creature)    -- ^ Function for determining target
    -> 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 = _crGroupCenter $ _creatureGroups w IM.! _crGroupID (_crGroup cr)
        p = f crTarg cenp cr

flockPointTargetR
    :: (Creature -> IM.IntMap Creature -> Creature -> Point2)
    -> (Creature -> World -> Maybe Creature)    -- ^ Function for determining target
    -> 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 w) is
        p = f crTarg crs cr

meleeHeadingMove
    :: Float -- ^ max turn speed
    -> Float -- ^ min turn speed
    -> Float -- ^ turn speed cutoff angle
    -> Float -- ^ move speed
    -> Point2 -- ^ target point
    -> Creature -- ^ start creature
    -> Creature -- ^ target creature
    -> [Impulse]
meleeHeadingMove maxta minta tacutoff speed tp cr tcr
    | dist tpos cpos < combinedRad + 5 
            && abs (_crDir cr - argV (tpos -.- cpos)) < tacutoff
            && _crMeleeCooldown 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 = _crPos cr
    tpos = _crPos tcr
    combinedRad = _crRad cr + _crRad tcr
    

mvPointMeleeTarg :: Point2 -> Creature -> Creature -> [Impulse]
mvPointMeleeTarg p cr crT
    | dist tpos cpos < combinedRad + 5 
            && abs (_crDir cr - argV (tpos -.- cpos)) < pi/4
            && _crMeleeCooldown 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 = _crPos cr
    tpos = _crPos crT
    combinedRad = _crRad cr + _crRad crT