{-# LANGUAGE TupleSections #-}
{-# LANGUAGE BangPatterns #-}
{- |
Basic helpers.
Consider splitting. -}
module Dodge.Base 
    where
import Dodge.Data
import Dodge.Zone
--import Dodge.Zone.Data
import Dodge.Base.Window
import Geometry
--import Picture
import qualified IntMapHelp as IM
import FoldableHelp

import Control.Lens
import qualified Control.Foldl as L
import Data.Monoid
import Data.Maybe
--import Data.Bifunctor
--import qualified Data.IntSet as IS
--import qualified Data.Set as S


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


safeHead :: [a] -> Maybe a
safeHead (x:_) = Just x
safeHead _ = Nothing

errorHead :: String -> [a] -> a
errorHead _ (x:_) = x
errorHead s [] = error s

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)

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

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 $ _znObjects $ _wallsZone w

creaturesNearPoint :: Point2 -> World -> IM.IntMap Creature
creaturesNearPoint p w =  IM.unions [f b $ f a $ _znObjects $ _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

creaturesNearPointI :: Int -> Point2 -> World -> IM.IntMap Creature
creaturesNearPointI n p w =  IM.unions [f b $ f a $ _znObjects $ _creaturesZone w | a<-[x-n..x+n] , b<-[y-n..y+n]]
  where 
    (x,y) = crZoneOfPoint 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 $ _znObjects $ _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
deleteIMInZone 
    :: Int      -- ^ First key
    -> Int      -- ^ Second key
    -> Int      -- ^ Third key
    -> IM.IntMap (IM.IntMap (IM.IntMap a)) 
    -> IM.IntMap (IM.IntMap (IM.IntMap a))
deleteIMInZone x y z = ix x . ix y %~ IM.delete z
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
{- | Finds unused projectile key. -}
newProjectileKey :: World -> Int
newProjectileKey = IM.newKey . _props
{- | Finds unused creature key. -}
newCrKey :: World -> Int
newCrKey = IM.newKey .  _creatures
{- | 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 
    = safeMinimumOn f
    $ 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 
    = safeMinimumOn f 
    $ 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
--
-- | Looks for overlap of a circle with walls.
-- If found, gives wall
overlapCircWallsReturnWall :: Point2 -> Float -> IM.IntMap Wall -> Maybe Wall
overlapCircWallsReturnWall p rad
    = L.fold (safeMinimumOnMaybeL (fmap (dist p) . f . _wlLine))
  where 
    f (a,b) = intersectSegSeg p (p -.- rad *.* vNormal (normalizeV (a -.- b))) a b

-- | Looks for any collision of a circle with walls.
-- If found, gives point and reflection velocity, reflection damped in normal.
-- note that in this version the circle can overlap the wall
collidePointAnyWallsReflect :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe (Point2,Point2)
collidePointAnyWallsReflect p1 p2 
    = getFirst
    . foldMap (First . findPoint . _wlLine)
  where 
    findPoint (x,y) = case intersectSegSeg p1 p2 x y of
        Just ip -> Just (ip +.+ normalizeV (vNormal (x -.- y)), reflectInParam 0.5 (x -.- y) (p2 -.- p1))
        Nothing -> Nothing

-- | Looks for collision of a point with walls.
-- If found, gives collision point
-- If not found, returns point
collidePointWalls :: Point2 -> Point2 -> IM.IntMap Wall -> Point2
collidePointWalls p1 p2 
    = foldr findPoint p2 . fmap _wlLine
  where
    findPoint (x,y) p = fromMaybe p $ intersectSegSeg p1 p x y

-- | Looks for first collision of a circle with walls.
-- If found, gives point and reflection velocity, reflection damped in normal.
-- note that the "intersection" point is the center of the circle flush against the wall
collideCircWalls' :: Point2 -> Point2 -> Float -> IM.IntMap Wall -> Maybe (Point2,Point2)
collideCircWalls' p1 p2 rad = either (const Nothing) Just . foldr findPoint (Left p2)
  where 
    findPoint wl eip = maybe eip Right $ doReflection (getp eip) $ shiftByRad $ _wlLine wl
    getp (Left p) = p
    getp (Right (p,_)) = p
    doReflection p (x,y) = case intersectSegSeg p1 p x y of
        Nothing -> Nothing
        Just ip -> Just (ip +.+ normalizeV (vNormal (x -.- y)), reflectInParam 0.5 (x -.- y) (p2 -.- p1))
    shiftByRad (a,b) =
        (g $ a +.+ rad *.* normalizeV (a -.-b)
        ,g $ b +.+ rad *.* normalizeV (b -.-a)
        )
      where
        g = ((rad *.* normalizeV (vNormal $ a -.- b)) +.+)

-- | 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 
    = safeMinimumOn (dist p1 . fst) 
    $ IM.mapMaybe 
        (( \(x:y:_) -> fmap 
            ((, reflectInParam 0.5 (x -.- y) (p2 -.- p1)) 
            . (+.+ normalizeV (vNormal (x -.- y)))
            ) 
            (intersectSegSeg p1 p2 x y)
         ) 
         . shiftByRad . _wlLine
        ) ws
  where 
    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 a list of lines.
-- If found, gives point and normal of wall.
--collidePointLines :: Point2 -> Point2 -> [Wall'] -> Maybe (Point2,Point2)
--collidePointLines p1 p2 ws 
--    = safeMinimumOn f
--    $ 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 normal of wall.
collidePointWallsNorm :: Point2 -> Point2 -> IM.IntMap Wall -> Maybe (Point2,Point2)
collidePointWallsNorm p1 p2 ws 
    = safeMinimumOn f 
    $ IM.mapMaybe (( \(x,y) -> intersectSegSeg p1 p2 x y <&> ( , vNormal $ x -.- y ) ) 
        . _wlLine) ws
  where 
    f (a,_) = magV (p1 -.- a)
-- | Returns the first creature, if any, that a point intersects with.
collidePointCreatures :: Point2 -> Point2 -> World -> Maybe Int
collidePointCreatures p1 p2 w 
    = fmap fst 
    . safeMinimumOn 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 
    . safeMinimumOn 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 
    . safeMinimumOn (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 
    . safeMinimumOn (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 
    . safeMinimumOn (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. 
 - Note no check on whether the wall is walkable. -}
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 
    = safeMinimumOn (dist p . _crPos)
    $ IM.filter (\cr -> dist p (_crPos cr) < r) $ _creatures w
{- | 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 
    = safeMinimumOn (dist p . _crPos)
    $ IM.filter (\cr -> pointInPolygon (_crPos cr) tri) $ _creatures w
 where 
    tri = 
        [p 
        ,p +.+ rotateV (dir-pi/4) (V2 x 0)
        ,p +.+ rotateV (dir+pi/4) (V2 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 
    = safeMinimumOn (dist p . _crPos)
    $ IM.filter (\cr -> pointInPolygon (_crPos cr) rec) $ _creatures w
  where 
    rec = [p, pR, pR1, pL1, pL ]
    pR  = p +.+ rotateV (dir - pi*(3/8)) (V2 (x/2) 0)
    pL  = p +.+ rotateV (dir + pi*(3/8)) (V2 (x/2) 0)
    pR1 = pR +.+ rotateV dir (V2 (x/2) 0)
    pL1 = pL +.+ rotateV dir (V2 (x/2) 0)
{- | Test whether a creature is in a polygon.  -}
crInPolygon :: Creature -> [Point2] -> Bool
crInPolygon cr = pointInPolygon (_crPos cr)

{- | Transform coordinates from world position to screen coordinates.  -}
worldPosToScreenNorm :: World -> Point2 -> Point2
worldPosToScreenNorm w = doWindowScale . doRotate . doZoom . doTranslate
  where
    doTranslate p       = p -.- _cameraCenter w
    doZoom p            = _cameraZoom w *.* p
    doRotate p          = rotateV (negate $ _cameraRot w) p
    doWindowScale (V2 x y) = V2
        ( x * 2 / getWindowX w)
        ( y * 2 / getWindowY w)
{- | Transform world coordinates to scaled screen coordinates. 
 - These have to be according to the size of the window to get actual screen positions.
 - This allows for line thicknesses etc to correspond to pixel sizes.-}
worldPosToScreen :: World -> Point2 -> Point2
worldPosToScreen w = doRotate . doZoom . doTranslate
  where
    doTranslate p       = p -.- _cameraCenter w
    doZoom p            = _cameraZoom w *.* p
    doRotate p          = rotateV (negate $ _cameraRot w) p
{- | Transform coordinates from the map position to 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 (V2 x y) = V2
        ( 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))

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)

dbArg :: (a -> a -> b) -> a -> b
{-# INLINE dbArg #-}
dbArg f x = f x x

-- TODO check whether this is simply the reader monad, flipped
dbArgChain :: (a -> b -> b) -> (a -> b -> b) -> a -> b -> b
dbArgChain f g x = f x . g x

