{-# LANGUAGE FlexibleInstances #-}

module Helper

where

import Data.Maybe
import Data.List
import Data.Ord
import Data.Function
import GHC.Exts

import FRP.Yampa
import FRP.Yampa.Geometry

import Object
import Message
import Command
import States
import Physics
import Global
import BasicTypes
import AL

-- *************************************************************************
--
-- Various geometric functions
--
-- *************************************************************************

spotToPoint :: Spot -> Point2 Double
spotToPoint (Spot x y) = Point2 x y
pointToSpot :: Point2 Double -> Spot
pointToSpot (Point2 x y) = Spot x y

distanceToSpot :: Spot -> Point2 Double -> Point2 Double -> Ordering
distanceToSpot (Spot x y) =
    comparing (distance (Point2 x y))

spotDistance :: Spot -> Point2 Double -> Double
spotDistance (Spot x1 y1) =
    distance (Point2 x1 y1) 
    
pointsForward :: RealFloat a => Vector3 a -> Team -> Bool
pointsForward v team =
   (team == Home && vector3Y v < 0) ||
   (team == Away && vector3Y v > 0)

tm :: TacticalPlayerMessage -> MessageBody
tm x = PlayerMessage $ TacticalPlayerMessage x
pm :: PhysicalPlayerMessage -> MessageBody
pm x = PlayerMessage $ PhysicalPlayerMessage x

otherTeam :: Team -> Team
otherTeam Home = Away
otherTeam Away = Home

lastPlayer :: (t, BallMsgParam) -> ObjId
lastPlayer ballState =
    case ballState of
        (_, BPWho oid _) -> oid
        (_, BPInit _ oid) -> oid
        (_, BPOutOfPlay _ _ _ oid) -> oid

teamMates :: ObjId -> [VisibleState] -> [VisibleState]
teamMates me vss =
    let team = vsTeam $ fetchVS vss me
    in [p | p@(VSPlayer {}) <- vss, vsTeam p == team, vsObjId p /= me]

teamPlayers :: Team -> [VisibleState] -> [VisibleState]
teamPlayers team vss = [p | p@(VSPlayer {vsTeam = team'}) <- vss, team' == team]

fetchGoalie :: Team -> [VisibleState] -> VisibleState
fetchGoalie team vss =
    head $ filter isGoalie $ teamPlayers team vss

isGoalie :: VisibleState -> Bool
isGoalie = (Goalie ==) . piPlayerRole . vsPlayerInfo

playerWithBall :: [VisibleState] -> Maybe ObjId
playerWithBall vss =
    case filter hasBall vss of
        [player] -> Just $ vsObjId player
        [] -> Nothing
        pls  -> error $ "Helper.hs/playerWithBall: too many players " ++ show (map vsObjId pls)

playerIsFree :: [VisibleState] -> VisibleState -> Bool
playerIsFree vss vs =
    let pos = vsPos vs
        team = vsTeam vs
        otherPlayers = teamPlayers (otherTeam team) vss
    in foldl' (\acc vso -> acc &&
                           distance pos (vsPos vso) > 5) True otherPlayers


homeValue :: Param -> Spot -> Double
homeValue param (Spot x y) =
  half x (pPositionFactorX param) (pPitchWidth param) +
    (pPitchLength param - y) * pPositionFactorY param

awayValue :: Param -> Spot -> Double
awayValue param (Spot x y) =
  homeValue param (Spot x (pPitchLength param - y))

half :: (Fractional a, Ord a) => a -> a -> a -> a
half u factor max_
  | u < (max_ / 2) = u * factor
   | otherwise = (max_ - u) * factor


bestFreePlayer :: Param -> [VisibleState] -> VisibleState -> Maybe VisibleState
bestFreePlayer param vss ballCarrier =
    let team = vsTeam ballCarrier
        me = vsObjId ballCarrier
        valFun = if team == Home then homeValue else awayValue
        freePlayers = filter (playerIsFree vss) $ teamMates me vss
    in if null freePlayers then Nothing
       else Just $ maximumBy (compare `on` valFun param . pointToSpot . projectP . vsPos) freePlayers

nearestNonAIPlayer :: Team -> [VisibleState] -> Position2 -> ObjId
nearestNonAIPlayer team vss pos =
    let players = [p | p@(VSPlayer {}) <- vss, vsTeam p == team, fst (vsPTState p) == TSNonAI ]
        nearest = minimumBy (\pl1 pl2 -> closerToPoint pos (projectP $ vsPos pl1) (projectP $ vsPos pl2)) players
    in vsObjId nearest

nearestAIPlayer :: Team -> [VisibleState] -> Position2 -> ObjId
nearestAIPlayer team vss pos =
    let players = [p | p@(VSPlayer {}) <- vss, vsTeam p == team, fst (vsPTState p) /= TSNonAI ]
        nearest = minimumBy (\pl1 pl2 -> closerToPoint pos (projectP $ vsPos pl1) (projectP $ vsPos pl2)) players
    in vsObjId nearest

nearestAIFieldPlayer :: Team -> [VisibleState] -> Position2 -> ObjId
nearestAIFieldPlayer team vss pos =
    let players = [p | p@(VSPlayer {}) <- vss, vsTeam p == team, fst (vsPTState p) /= TSNonAI,
                         (piPlayerRole . vsPlayerInfo) p /= Goalie ]
        nearest = minimumBy (\pl1 pl2 -> closerToPoint pos (projectP $ vsPos pl1) (projectP $ vsPos pl2)) players
    in vsObjId nearest

nearestPlayer :: Team -> [VisibleState] -> Position2 -> ObjId
nearestPlayer team vss pos =
    let players = [p | p@(VSPlayer {}) <- vss, vsTeam p == team]
        nearest = minimumBy (\pl1 pl2 -> closerToPoint pos (projectP $ vsPos pl1) (projectP $ vsPos pl2)) players
    in vsObjId nearest

closerToPoint :: (Ord a, AffineSpace p v a) => p -> p -> p -> Ordering
closerToPoint p p1 p2 =
    if distance p p1 < distance p p2 then LT else GT

fetchVS :: [VisibleState] -> ObjId -> VisibleState
fetchVS vss = fromJust . flip getObjVS vss

getObjVS :: ObjId -> [VisibleState] -> Maybe VisibleState
getObjVS _ [] = Nothing
getObjVS oid (vs:vss) =
    if vsObjId vs == oid then Just vs else getObjVS oid vss

fetchGameVS :: [VisibleState] -> VisibleState
fetchGameVS [] = error "Helper/fetchGameVS: No game in Visible States"
fetchGameVS (v:vs) =
    case v of
        VSGame {} -> v
        _ -> fetchGameVS vs

fetchBallVS :: [VisibleState] -> VisibleState
fetchBallVS [] = error "Helper/fetchBallVS: No ball in Visible States"
fetchBallVS (v:vs) =
    case v of
        VSBall {} -> v
        _ -> fetchBallVS vs

fetchBallCarrier :: [VisibleState] -> Maybe VisibleState
fetchBallCarrier vss =
    let (s, sp) = vsBallState ball
        ball = fetchBallVS vss
    in if s `elem` [BSControlled, BSControlledOOP, BSControlledGoalie]
           then Just (fetchVS vss (fromBPWho sp))
           else Nothing

ballId :: ALOut -> ObjId
ballId (AL []) = error "Helper.hs/ballId: No Ball in Object Output"
ballId (AL ((xId, xOO):xs)) =
   case xOO of
       ObjOutput (OOSBall {}) _ _ _ -> xId
       _ -> ballId (AL xs)

gameId :: ALOut -> ObjId
gameId (AL []) = error "Helper.hs/gameId: No Game in Object Output"
gameId (AL ((xId, xOO):xs)) =
   case xOO of
       ObjOutput (OOSGame {}) _ _ _ -> xId
       _ -> gameId (AL xs)
                                                     --  time  possession score1 score2
hasBall :: VisibleState -> Bool
hasBall vs = isPlayer vs && (fst . vsPBState) vs == PBSInPossession

offsiteFrontier :: Param -> ObjId -> [VisibleState] -> Point2 Position
offsiteFrontier param me vss =
-- yield the a position for the player on the offsite frontiert. needs at least 2 players in every team
    let myself = fetchVS vss me
        myTeam = vsTeam myself
        Point3 myXPos _ _ = vsPos myself
        others = teamPlayers (otherTeam myTeam) vss
        reverser = if myTeam == Home then id else reverse
                         -- Achtung: Auf-/absteigend sortieren je nach Home / Away!
        Point3 _ y _ = vsPos . head . tail . reverser $ sortWith (point3Y . vsPos) others
        half_ = pPitchLength param / 2
        yAdjust = if myTeam == Home then min y half_ + 1 else max y half_ - 1
    in Point2 myXPos yAdjust

adjustForOffsite :: Param -> Point2 Position -> ObjId -> [VisibleState] -> Point2 Position
adjustForOffsite param p@(Point2 _ y) me vss =
    let pOff@(Point2 _ yOff) = offsiteFrontier param me vss
        myself = fetchVS vss me
        myTeam = vsTeam myself
        ballCarrier = hasBall myself
    in
        if not ballCarrier &&
             ((myTeam == Home && y < yOff) ||
               (myTeam == Away && y > yOff))
        then pOff else p

basePosition :: Param -> ObjId -> [VisibleState] -> Team -> TacticalStateParam
basePosition param me vss attacker =
-- yield me's optimal position in relation to the ball and his state (attacking or defending)
    let pI = vsPlayerInfo $ fetchVS vss me
        defensivePos = piBasePosDefense pI
        offensivePos = piBasePosOffense pI
        myTeam = vsTeam $ fetchVS vss me
        ball = fetchBallVS vss
        posBall = projectP . vsPos $ ball
        adjust = posBall .-. pitchCenter param
        adjust' = vector2 (pHorizontalShiftRatio param * vector2X adjust) (pVerticalShiftRatio param * vector2Y adjust)
--        defPos = limitPosition (border, pitchWidth + border, border + lineEnds, pitchLength + border - lineEnds) (defensivePos .+^ adjust')
        defPos = limitPosition (0, pPitchWidth param, pLineEnds param, pPitchLength param - pLineEnds param) (defensivePos .+^ adjust')
        offPos = adjustForOffsite param offensivePos me vss
    in TacticalStateParam (Just $ if myTeam == attacker then offPos else defPos)
                          Nothing
                          False
                          Nothing
                          Nothing
                          Nothing
                          Nothing

-- *************************************************************************
--
-- Various geometric functions
--
-- *************************************************************************

limitPosition :: RealFloat a => (a, a, a, a) -> Point2 a -> Point2 a
limitPosition (xmin, xmax, ymin, ymax) (Point2 px py) =
    Point2 (minmax xmin xmax px) (minmax ymin ymax py)
    where minmax min_ max_ z = if z < min_ then min_ else if z > max_ then max_ else z

noPoint :: Point2 Double
noPoint = Point2 (-1) (-1)
zeroVel :: Vector3 Double
zeroVel = vector3 0 0 0

sameDirection :: Param -> Velocity2 -> Velocity2 -> Bool
sameDirection param u v =
   abs(vector2Rho u) > pEps param &&
   abs((vector2Rho u + vector2Rho v) - vector2Rho (u ^+^ v)) < pEps param

inOneSecond :: AffineSpace p v a => p -> v -> p
inOneSecond pos vel = pos .+^ vel

(.+!) :: Point2 Double -> Double -> Point3 Double
(Point2 x y) .+! z = Point3 x y z


(^+!) :: Vector2 Double -> Double -> Vector3 Double
v ^+! z =
  let x = vector2X v
      y = vector2Y v
  in vector3 x y z

project :: Velocity3 -> Velocity2
project v = vector2 x y
  where x = vector3X v
        y = vector3Y v

projectP :: Position3 -> Position2
projectP v = Point2 x y
  where x = point3X v
        y = point3Y v

brake :: Param -> Velocity3 -> Double -> Velocity3
brake param v a  =
  vector3 (-b*x) (-b*y) (pGravity param)
  where x = vector3X v
        y = vector3Y v
        b = a / vector2Rho (vector2 x y)

halfPi :: Double
halfPi = pi / 2

(^-.) :: RealFloat a => Point2 a -> Vector2 a -> Point2 a
(Point2 x y) ^-. v = Point2 (vector2X v - x) (vector2Y v - y)

normTheta :: (Floating a, Ord a) => a -> a
normTheta x
    | x < (- pi) = x + 2 * pi
    | x > pi = x - 2 * pi
    | otherwise = x

lookTo :: AffineSpace p v a => p -> p -> p
lookTo p0 p1 =
    p0 .+^ (0.0001 *^ normalize (p1 .-. p0))

towards :: Spot -> Spot -> Vector3 Double
towards curr dest =
    let delta = 20 *^ normalize (spotToPoint dest .-. spotToPoint curr)
    in delta ^+! 0

turnBy :: Vector2 Double -> Vector2 Double -> Vector2 Double
u `turnBy` v =
    fromPolar (vector2Theta u - vector2Theta v) (vector2Rho u)

isLeftFrom :: Vector2 Double -> Vector2 Double -> Bool
u `isLeftFrom` v =
    normTheta (vector2Theta (u `turnBy` v)) >= 0

isRigthFrom :: Vector2 Double -> Vector2 Double -> Bool
u `isRigthFrom` v = v `isLeftFrom` u

sameDirAs :: Vector2 Double -> Vector2 Double -> Bool
u `sameDirAs` v =
    let theta = normTheta $ vector2Theta (u `turnBy` v)
    in
        (theta > 0 && theta < pi/2) || (theta < 0 && theta > -(pi/2))

toPolar :: RealFloat t => t -> t -> (t, t)
toPolar x y =
   (atan2 y x, sqrt ((x*x) + (y*y)))

getAngle :: RealFloat a => Vector2 a -> a
getAngle v = atan2 (vector2Y v) (vector2X v)

fromPolar :: RealFloat a => a -> a -> Vector2 a
fromPolar theta rho =
    vector2 (rho * cos theta) (rho * sin theta)

fromPolar3 :: RealFloat a => a -> a -> a -> Vector3 a
fromPolar3 theta rho =
    vector3 (rho * cos theta) (rho * sin theta)

limit :: (Ord a, VectorSpace v a) => a -> v -> v
limit max_ v =
    let len = norm v
    in
      if len > max_ then (max_/len) *^ v else v

mirrorPoint :: RealFloat a => Point2 a -> Point2 a -> Point2 a
mirrorPoint (Point2 x y) (Point2 ax ay) =
    Point2 (mirrorAt x ax) (mirrorAt y ay)

mirrorAt :: Num a => a -> a -> a
mirrorAt x axis = 2 * axis - x

sqr :: Num a => a -> a
sqr x = x * x
-- *************************************************************************
--
-- Parameters
--
-- *************************************************************************

awayGoalCenter :: Param -> Point2 Double
awayGoalCenter param = Point2 (pPitchWidth param / 2) 0
homeGoalCenter :: Param -> Point2 Double
homeGoalCenter param = Point2 (pPitchWidth param / 2) (pPitchLength param)
pitchCenter :: Param -> Point2 Double
pitchCenter param = Point2 (pPitchWidth param / 2) (pPitchLength param / 2)

-- *************************************************************************
--
-- Messages and commands
--
-- *************************************************************************

comToMsg :: Command -> [MessageBody]
comToMsg (CmdKickHigh dT) = [pm (PPTLoseMe, BSPRelease dT RTHigh)]
comToMsg (CmdKickLow dT) = [pm (PPTLoseMe, BSPRelease dT RTLow)]
comToMsg (CmdPassHigh dT) = [pm (PPTLoseMe, BSPPass dT RTHigh Nothing)]
comToMsg (CmdPassLow dT) = [pm (PPTLoseMe, BSPPass dT RTLow Nothing)]
comToMsg CmdFlipHigh = [pm (PPTLoseMe, BSPPass 1 RTHigh Nothing)]
comToMsg CmdFlipLow = [pm (PPTLoseMe, BSPPass 1 RTLow Nothing)]
--comToMsg pos CmdMoveForward = [tm (TPTMoveTo, TacticalStateParam (Just $ pos .+^ vector2 0 (-20))
--                                                                       Nothing False Nothing
--                                                                       Nothing Nothing)]
comToMsg _ = []

-- *************************************************************************
--
-- Various generic list and tupel functions
--
-- *************************************************************************

collect :: (Eq a) => [(a,b)] -> [(a, [b])]
collect [] = []
collect ((a, b) : rest) =
   (a, b:fetch a rest) : collect (remove a rest)

fetch :: (Eq a) => a -> [(a,b)] -> [b]
fetch a = map snd . filter (\(a',_) -> a == a')

remove :: Eq a => a -> [(a, t)] -> [(a, t)]
remove a = filter (\(a',_) -> a /= a')

mergeList :: (Eq a) => [(a, [b])] -> [(a, [b])]
mergeList =
   map (\(a,bs) -> (a, concat bs)) . collect

fst3 :: (t, t1, t2) -> t
fst3  (a, _, _) = a
snd3 :: (t, t1, t2) -> t1
snd3  (_, b, _) = b
thrd3 :: (t, t1, t2) -> t2
thrd3 (_, _, c) = c

fromRight :: Either t t1 -> t1
fromRight (Right x) = x