{-# LANGUAGE TupleSections #-}
{-# LANGUAGE BangPatterns #-}
{- |
Basic helpers.
Consider splitting. -}
module Dodge.Base 
    ( module Dodge.Base
    , module Dodge.Base.Zone
    , module Dodge.Base.Window
    , module Dodge.Base.Collide
    )
    where
import Dodge.Data
import Dodge.Base.Zone
import Dodge.Base.Window
import Dodge.Base.Collide
import Geometry
import Picture

import Control.Lens

--import Control.Monad.State
import Data.List
--import Data.Function
import Data.Maybe
--import Data.Bifunctor
import qualified Data.IntMap.Strict as IM
--import qualified Data.IntSet as IS
--import qualified Data.Set as S

leftPad :: Int -> a -> [a] -> [a]
leftPad i x xs = reverse $ take i $ reverse (take i xs) ++ repeat x

rightPad :: Int -> a -> [a] -> [a]
rightPad i x xs = take i $ xs ++ repeat x

midPad :: Int -> a -> [a] -> [a] -> [a]
midPad i x xs ys = xs ++ replicate j x ++ ys
  where 
    j = i - (length xs + length ys)

midPadL :: Int -> a -> [a] -> [a] -> [a]
midPadL i x xs ys = take j (xs ++ repeat x) ++ ys
  where 
    j = i - length ys

{- | Implementation copied from 
 - https://hackage.haskell.org/package/utility-ht-0.0.16/docs/src/Data.List.HT.Private.html#takeUntil
 -}
takeUntil :: (a -> Bool) -> [a] -> [a]
takeUntil p = foldr (\x xs -> x : if p x then [] else xs) []

you :: World -> Creature
you w = _creatures w IM.! _yourID w

aCrPos :: Int -> World -> Point2
aCrPos i w = _crPos $ _creatures w IM.! i

yourItem :: World -> Item
yourItem w = _crInv (you w) IM.! _crInvSel (you w)

crItem :: World -> Int -> Item
crItem w cid = _crInv cr IM.! _crInvSel cr
  where
    cr = _creatures w IM.! cid

yourItemRef
    :: Applicative f
    => World
    -> (Item -> f Item)
    -> World
    -> f World
yourItemRef w = creatures . ix (_yourID w) . crInv . ix (_crInvSel (you w))

wallNormal :: Wall -> Point2
wallNormal wl = normalizeV . vNormal $ a -.- b
  where 
    (a,b) = _wlLine wl

wallsOnLine :: Point2 -> Point2 -> IM.IntMap Wall -> [Wall]
wallsOnLine p1 p2 ws = hitWalls
  where 
    hitPoint w = uncurry (intersectSegSeg' p1 p2) (_wlLine w)
    hitWalls = filter (isJust . hitPoint) (IM.elems ws)

wallOnLine :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe Wall
wallOnLine p1 p2 ws 
    = listToMaybe $ sortBy f hitWalls
  where 
    hitPoint w = uncurry (intersectSegSeg' p1 p2) (_wlLine w)
    hitWalls = filter (isJust . hitPoint) (IM.elems ws)
    f w1 w2 = compare (magV (p1 -.- fromJust (hitPoint w1))) (magV (p1 -.- fromJust (hitPoint w2)))

wallsOnCirc :: Point2 -> Float -> IM.IntMap Wall -> IM.IntMap Wall
wallsOnCirc p r = IM.filter f
  where 
    f wl = uncurry circOnSeg (_wlLine wl) p r

allWalls :: World -> IM.IntMap Wall
allWalls w = IM.unions $ concatMap IM.elems $ IM.elems $ _wallsZone w

creaturesNearPoint :: Point2 -> World -> IM.IntMap Creature
creaturesNearPoint p w =  IM.unions [f b $ f a $ _creaturesZone w | a<-[x-1,x,x+1] , b<-[y-1,y,y+1]]
  where 
    (x,y) = crZoneOfPoint p
    f i m = case IM.lookup i m of 
        Just val -> val
        _        -> IM.empty

cloudsNearPoint :: Point2 -> World -> IM.IntMap Cloud
cloudsNearPoint p w =  IM.unions [f b $ f a $ _cloudsZone w | a<-[x-1,x,x+1] , b<-[y-1,y,y+1]]
  where 
    (x,y) = cloudZoneOfPoint p
    f i m = case IM.lookup i m of 
        Just val -> val
        _        -> IM.empty
-- possible BUG, occurs when used in thingsHitLongLine
creaturesAlongLine :: Point2 -> Point2 -> World -> IM.IntMap Creature
--creaturesAlongLine a b w = IM.unions [f y $ f x $ _creaturesZone w | (x,y) <- zoneOfLine a b]
--    where f i m = case IM.lookup i m of Just val -> val
--                                        _        -> IM.empty
creaturesAlongLine a b w = IM.foldrWithKey' g IM.empty kps
    where g x s = IM.union (IM.unions (IM.restrictKeys (f x $ _creaturesZone w) s))
          kps = zoneOfLineIntMap a b
          f i m = case IM.lookup i m of Just val -> val
                                        _        -> IM.empty
{- | Expands a line out to a given thickness. -}
lineGeom :: Float -> Point2 -> Point2 -> [Point2]
lineGeom t x y
    | x == y    = []
    | otherwise = [x +.+ n x y, x -.- n x y, y +.+ n x y, y -.- n x y]
  where 
    n a b      = (t*0.5) *.* errorNormalizeV 4200 (vNormal (a -.- b))
{- | A triangular wedge thick at the first point and
 - tapering off to the second. -}
wedgeGeom 
    :: Float    -- Thickness
    -> Point2 
    -> Point2 
    -> [Point2]
wedgeGeom t x y 
    | x == y    = []
    | otherwise = [x +.+ n x y, x -.- n x y, y]
  where 
    n a b = (t*0.5) *.* errorNormalizeV 4200 (vNormal (a -.- b))

insertInZoneWith 
    :: Int          -- ^ First Key
    -> Int          -- ^ Second Key
    -> (a -> a -> a) -- ^ Combining function
    -> a            -- ^ Value to insert
    -> IM.IntMap (IM.IntMap a) 
    -> IM.IntMap (IM.IntMap a)
insertInZoneWith x y fun obj = IM.insertWith f x $ IM.singleton y obj
  where
    f _ = IM.insertWith fun y obj
{- | I believe this overwrites the value if it already exists, but not sure. -}
insertIMInZone 
    :: Int      -- ^ First key
    -> Int      -- ^ Second key
    -> Int      -- ^ Third key
    -> a        -- ^ Item to insert
    -> IM.IntMap (IM.IntMap (IM.IntMap a)) 
    -> IM.IntMap (IM.IntMap (IM.IntMap a))
insertIMInZone x y obid obj = IM.insertWith f x $ IM.singleton y $ IM.singleton obid obj
  where 
    f _ = IM.insertWith g y $ IM.singleton obid obj
    g _ = IM.insert obid obj
adjustIMZone 
    :: (a -> a) -- ^ Update function
    -> Int      -- ^ First key
    -> Int      -- ^ Second key
    -> Int      -- ^ Third key
    -> IM.IntMap (IM.IntMap (IM.IntMap a)) 
    -> IM.IntMap (IM.IntMap (IM.IntMap a))
adjustIMZone f x y n = IM.adjust f' x
  where 
    f'  = IM.adjust f'' y
    f'' = IM.adjust f n
{- | Find a key value one higher than any key in the map, or zero if the map is
 - empty -}
newKey :: IM.IntMap a -> Int
newKey = maybe 0 ((+ 1) . fst) . IM.lookupMax
{- | Finds unused projectile key. -}
newProjectileKey :: World -> Int
newProjectileKey = newKey . _projectiles
{- | Finds unused creature key. -}
newCrKey :: World -> Int
newCrKey = newKey .  _creatures
{- | Insert an element with some new key. -}
insertNewKey :: a -> IM.IntMap a -> IM.IntMap a
insertNewKey x m = case IM.lookupMax m of
    Nothing     -> IM.singleton 0 x
    Just (k,_)  -> IM.insert (k+1) x m
{- | TODO: determine precisely what this does. -}
reflectPointCreature :: Point2 -> Point2 -> Creature -> Maybe (Point2, Point2, Int)
reflectPointCreature p1 p2 cr = case collidePointCirc p1 p2 (_crRad cr) (_crPos cr) of
    Nothing -> Nothing
    Just _ -> Just 
        ( p1
        , errorNormalizeV 35 (ssaTriPoint p2 (_crPos cr) p1 (_crRad cr) -.- _crPos cr) 
            +.+ (_crPos cr -.- _crOldPos cr)
        , _crID cr)
{- | TODO: determine precisely what this does. -}
reflectPointCreatures :: Point2 -> Point2 -> IM.IntMap Creature -> Maybe (Point2,Point2,Int)
reflectPointCreatures p1 p2 cs 
    = listToMaybe 
    . sortOn f
    . IM.elems 
    $ IM.mapMaybe (reflectPointCreature p1 p2) cs
  where 
    f (a,_,_) = magV (a -.- p1)
{- | TODO: determine precisely what this does. -}
reflectCircCreature 
    :: Float    -- ^ Radius
    -> Point2   -- ^ Start point
    -> Point2   -- ^ End point
    -> Creature 
    -> Maybe (Point2, Point2, Int)
reflectCircCreature rad p1 p2 cr = case collidePointCirc p1 p2 (rad + _crRad cr) (_crPos cr) of
    Nothing -> Nothing
    Just _ -> Just 
        ( p1
        , errorNormalizeV 37 (ssaTriPoint p2 (_crPos cr) p1 (_crRad cr) -.- _crPos cr) 
            +.+ (_crPos cr -.- _crOldPos cr)
        , _crID cr
        )
{- | TODO: determine precisely what this does. -}
reflectCircCreatures 
    :: Float    -- ^ Radius
    -> Point2   -- ^ Start point
    -> Point2   -- ^ End point
    -> IM.IntMap Creature 
    -> Maybe (Point2,Point2,Int)
reflectCircCreatures rad p1 p2 cs 
    = listToMaybe 
    . sortOn f 
    . IM.elems 
    $ IM.mapMaybe (reflectCircCreature rad p1 p2) cs
  where 
    f (a,_,_) = magV (a -.- p1)
-- | collides a point with forcefields
-- if found, returns point of collision, deflection if required, and the id
collidePointFFs :: a
collidePointFFs = undefined
collidePointFF  :: a
collidePointFF  = undefined
--
--collidePointFFs :: Point2 -> Point2 -> StdGen -> IM.IntMap ForceField 
--                    -> Maybe (Point2,(Maybe (Point2,StdGen),Int))
--collidePointFFs p1 p2 g fs = listToMaybe $ sortBy f $ IM.elems 
--                            $ IM.mapMaybe (collidePointFF p1 p2 g) fs
--    where f (a,_) (b,_) = compare (magV (p1 -.- a)) (magV (p1 -.- b))
--
--collidePointFF :: Point2 -> Point2 -> StdGen -> ForceField 
--                    -> Maybe (Point2,(Maybe (Point2,StdGen),Int))
--collidePointFF p1 p2 g ff = fmap f ip
--    where (p3:p4:_) = _ffLine ff
--          ip  = intersectSegSeg' p1 p2 p3 p4
--          ref = (_ffDeflect ff) <*> Just g <*> Just (p2 -.- p1) <*> Just ff
--          f p = (p, (ref, _ffID ff))
--

-- | Looks for first collision of a circle with walls.
-- If found, gives point and reflection velocity, reflection damped in normal.
collideCircWalls' :: Point2 -> Point2 -> Float -> IM.IntMap Wall -> Maybe (Point2,Point2)
collideCircWalls' p1 p2 rad ws 
    = listToMaybe 
    . sortOn f 
    . IM.elems 
    $ IM.mapMaybe 
        (( \(x:y:_) -> fmap 
            ((, reflectInParam 0.5 (x -.- y) (p2 -.- p1)) 
            . (+.+ errorNormalizeV 40 (vNormal (x -.- y)))
            ) 
            (intersectSegSeg' p1 p2 x y)
         ) 
         . shiftByRad . _wlLine
        ) ws
  where 
    f (a,_) = magV (p1 -.- a)
    shiftByRad (a,b) = map ((rad *.* normalizeV (vNormal $ a -.- b)) +.+)
        [a +.+ rad *.* normalizeV (a -.-b)
        ,b +.+ rad *.* normalizeV (b -.-a)
        ]
-- this shifts the wall out, and for outer corners extends the wall
-- not sure what this does for inner corners, hopefully won't cause a problem
-- the alternative would be to separately bounce off corner points...
-- unfortunately, doesn't allow for collisions when the circle spawns on the
-- wall

-- | Looks for first collision of a point with walls.
-- If found, gives point and normal of wall.
collidePointWallsNorm :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe (Point2,Point2)
collidePointWallsNorm p1 p2 ws 
    = listToMaybe 
    . sortOn f 
    . IM.elems 
    $ IM.mapMaybe (( \(x,y) -> intersectSegSeg' p1 p2 x y <&> ( , vNormal $ x -.- y ) ) 
        . _wlLine) ws
  where 
    f (a,_) = magV (p1 -.- a)
{- | Looks for first collision of a point with walls.
If found, gives point and colour of wall.
-}
collidePointWallsCol :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe (Point2,Color)
collidePointWallsCol p1 p2 ws 
    = listToMaybe 
    . sortOn f 
    . IM.elems 
    $ IM.mapMaybe ( (\(m, c) -> m <&> (, c))
        . (\w -> (uncurry (intersectSegSeg' p1 p2) (_wlLine w), _wlColor w))) ws
  where 
    f (a,_) = magV (p1 -.- a)
{- | Looks for first collision of a point with walls.
If found, gives point, and normal and colour of wall.
-}
collidePointWallsNormCol :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe (Point2,Point2,Color)
collidePointWallsNormCol p1 p2 ws 
    = listToMaybe 
    . sortOn f 
    . IM.elems 
    $ IM.mapMaybe m ws
  where 
    f (a,_,_) = magV $ p1 -.- a
    m w = 
        let (x,y) = _wlLine w
        in intersectSegSeg' p1 p2 x y <&> (, vNormal (x -.- y), _wlColor w)
-- | Returns the first creature, if any, that a point intersects with.
collidePointCreatures :: Point2 -> Point2 -> World -> Maybe Int
collidePointCreatures p1 p2 w 
    = fmap fst 
    . listToMaybe 
    . sortOn snd
    . IM.toList 
    . IM.mapMaybe (\x -> collidePointCirc' p1 p2 (_crRad x) (_crPos x)) 
    $_creatures w
-- | As for 'collidePointCreatures', only increases the radius of creatures by a
--fixed amount, thus collides a moving circle with creaures.
collideCircCreatures :: Point2 -> Point2 -> Float -> World -> Maybe Int
collideCircCreatures p1 p2 rad w 
    = fmap fst 
    . listToMaybe 
    . sortOn snd
    . IM.toList 
    . IM.mapMaybe (\x -> collidePointCirc' p1 p2 (rad + _crRad x) (_crPos x)) 
    $ _creatures w
-- | Returns the first creature id, if any, that a point intersects with, gives point
--in creature on line.
collidePointCrsPoint :: Point2 -> Point2 -> World -> Maybe (Point2,Int)
collidePointCrsPoint p1 p2 w 
    = fmap f 
    . listToMaybe 
    . sortOn (snd . snd)
    . IM.toList 
    . IM.mapMaybe (\x -> collidePointCirc'' p1 p2 (_crRad x) (_crPos x)) 
    $ _creatures w
  where 
    f (cID,(p,_)) = (p,cID)
{- | Finds the first creature hit on a line.
Maybe evaluates the creature id and hit point. -}
collideCircCrsPoint :: Point2 -> Point2 -> Float -> World -> Maybe (Point2,Int)
collideCircCrsPoint p1 p2 rad w 
    = fmap f 
    . listToMaybe 
    . sortOn (snd . snd)
    . IM.toList 
    . IM.mapMaybe (\x -> collidePointCirc'' p1 p2 (rad + _crRad x) (_crPos x)) 
    $ _creatures w
  where 
    f (cID,(p,_)) = (p,cID)
-- | Makes a creature not hittable.
collidePointCrsWithoutPoint :: Int -> Point2 -> Point2 -> World -> Maybe (Point2,Int)
collidePointCrsWithoutPoint cid p1 p2 w 
    = fmap f 
    . listToMaybe 
    . sortOn (snd . snd)
    . IM.toList 
    . IM.mapMaybe (\x -> collidePointCirc'' p1 p2 (_crRad x) (_crPos x)) 
    . IM.delete cid 
    $ _creatures w
  where 
    f (cID,(p,_)) = (p,cID)
{- | Test if a circle collides with any wall. -}
circOnSomeWall :: Point2 -> Float -> World -> Bool
circOnSomeWall p rad w 
    = any (\(x,y) -> circOnSeg x y p rad) 
    . fmap _wlLine 
    . IM.elems 
    $ wallsNearPoint p w
{- | Test whether there is a creature of weight 4 or greater near a line. -}
isHeavyCrNearLine 
    :: Float 
    -> [Point2] 
    -> World 
    -> Bool
isHeavyCrNearLine d (p1:p2:_) w 
    = any (\c -> circOnSeg p1 p2 (_crPos c) (d + _crRad c)) 
    . IM.filter (\cr -> _crMass cr > 4) 
    $ _creatures w
isHeavyCrNearLine _ _ _ = error "Testing whether creature is near empty line"
{- | Adds the distance to the creature radius, tests whether the center is in
the circle of this size centered at the point -}
crsNearPoint :: Float -> Point2 -> World -> Bool
crsNearPoint d p w = any (\c -> dist (_crPos c) p < (d + _crRad c)) (_creatures w)
{- | Produce an unordered list of creatures on a line. -}
crsOnLine :: Point2 -> Point2 -> World -> [Creature]
crsOnLine p1 p2 w 
    = IM.elems 
    . IM.filter (\cr -> circOnSeg p1 p2 (_crPos cr) (_crRad cr))
    $ _creatures w
{- | Produce an unordered list of creatures on a wide line. -}
crsOnThickLine :: Float -> Point2 -> Point2 -> World -> [Creature]
crsOnThickLine thickness p1 p2 w 
    = IM.elems 
    . IM.filter (\cr -> circOnSeg p1 p2 (_crPos cr) (_crRad cr + thickness))
    $ _creatures w
{- | Find 'Maybe' the closest creature to a point, within a circle.
 -}
nearestCrInRad :: Point2 -> Float -> World -> Maybe Creature
nearestCrInRad p r w = 
    let crs = IM.filter (\cr -> dist p (_crPos cr) < r) $ _creatures w
        sortedCrs = sortOn (dist p . _crPos) $ IM.elems crs
    in listToMaybe sortedCrs
{- | Find 'Maybe' the closest creature in front of a point in a right-angle-triangle shape.
 -}
nearestCrInTri 
    :: Point2 
    -> Float    -- ^ Direction (radians +ve anticlockwise from x-axis).
    -> Float    -- ^ Distance. 
    -> World -> Maybe Creature
nearestCrInTri p dir x w = 
    let crs = IM.filter (\cr -> errorPointInPolygon 1 (_crPos cr) tri) $ _creatures w
        sortedCrs = sortOn (dist p . _crPos) $ IM.elems crs
    in listToMaybe sortedCrs
 where 
    tri = 
        [p 
        ,p +.+ rotateV (dir-pi/4) (x,0)
        ,p +.+ rotateV (dir+pi/4) (x,0)
        ]
{- | Find 'Maybe' the closes creature in front of a point in a given direction for
a given distance.
The shapes within which creatures are searched are a triangle then rectangle.  -}
nearestCrInFront 
    :: Point2
    -> Float    -- ^ Direction (radians +ve anticlockwise from x-axis).
    -> Float    -- ^ Distance.
    -> World -> Maybe Creature
nearestCrInFront p dir x w = 
    let crs = IM.filter (\cr -> errorPointInPolygon 2 (_crPos cr) rec) $ _creatures w
        sortedCrs = sortOn (dist p . _crPos) $ IM.elems crs
    in listToMaybe sortedCrs
  where 
    rec = [p, pR, pR1, pL1, pL ]
    pR  = p +.+ rotateV (dir - pi*(3/8)) (x/2,0)
    pL  = p +.+ rotateV (dir + pi*(3/8)) (x/2,0)
    pR1 = pR +.+ rotateV dir (x/2,0)
    pL1 = pL +.+ rotateV dir (x/2,0)
{- | Test whether a creature is in a polygon.  -}
crInPolygon :: Creature -> [Point2] -> Bool
crInPolygon cr = errorPointInPolygon 3 (_crPos cr)
{- | Transform coordinates from world position to normalised screen coordinates.  -}
worldPosToScreen :: World -> Point2 -> Point2
worldPosToScreen w = doWindowScale . doRotate . doZoom . doTranslate
  where
    doTranslate p       = p -.- _cameraCenter w
    doZoom p            = _cameraZoom w *.* p
    doRotate p          = rotateV (negate $ _cameraRot w) p
    doWindowScale (x,y) =   
        ( x * 2 / getWindowX w
        , y * 2 / getWindowY w
        )
{- | Transform coordinates from the map position to normalised screen
coordinates.  -}
cartePosToScreen :: World -> Point2 -> Point2
cartePosToScreen w = doWindowScale . doRotate . doZoom . doTranslate
  where
    doTranslate p       = p -.- _carteCenter w
    doZoom p            = _carteZoom w *.* p
    doRotate p          = rotateV (negate $ _carteRot w) p
    doWindowScale (x,y) =   
        ( x * 2 / getWindowX w
        , y * 2 / getWindowY w
        )
{- | The mouse position in world coordinates.  -}
mouseWorldPos :: World -> Point2
mouseWorldPos w = _cameraCenter w +.+ (1/_cameraZoom w) *.* rotateV (_cameraRot w) (_mousePos w)
{- | The mouse position in map coordinates -}
mouseCartePos :: World -> Point2
mouseCartePos w = _carteCenter w +.+ (1/_carteZoom w) *.* rotateV (_carteRot w) (_mousePos w)
{- | Create a logistic function given three parameters.  -}
logistic :: Float -> Float -> Float -> (Float -> Float)
logistic x0 l k x = l / (1 + exp (k*(x0 - x)))
{- | given a target and a start point, shift toward the end point by a given
amount.
If close enough, end up on the end point -}
mvPointTowardAtSpeed 
    :: Float    -- ^ Speed.
    -> Point2   -- ^ End point.
    -> Point2   -- ^ Start point.
    -> Point2
mvPointTowardAtSpeed speed !ep !p 
    | dist p ep < speed = ep
    | otherwise         = p +.+ speed *.* normalizeV (ep -.- p)
{- | given a target and a start point, shift toward the end point by 1.
If close enough, end up on the end point -}
mvPointToward 
    :: Point2 -- ^ End point.
    -> Point2 -- ^ Start point.
    -> Point2
mvPointToward !ep !p 
    | dist p ep < 1 = ep
    | otherwise     = p +.+ normalizeV (ep -.- p)

isAnimate :: Creature -> Bool
{-# INLINE isAnimate #-}
isAnimate cr = case _crActionPlan cr of
    Inanimate -> False
    _   -> True

sigmoid :: Floating a => a -> a
sigmoid x = x/sqrt(1+x^(2::Int))

{- | In order to force a list, apply with seq. -}
forceSpine :: [a] -> ()
forceSpine = foldr (const id) ()

forceList :: [a] -> [a]
forceList l = seq (forceSpine l) l

normalizeAnglePi :: Float -> Float
normalizeAnglePi angle 
    | normalizeAngle angle > pi = normalizeAngle angle - 2*pi
    | otherwise                 = normalizeAngle angle

-- | Taken from online, splits a list into its even and odd elements
evenOddSplit :: [a] -> ([a],[a])
evenOddSplit = foldr f ([],[])
  where 
    f a (ls,rs) = (rs, a : ls)