module Dodge.Creature.Impulse.Movement (
    creatureTurnToward,
    creatureTurnTowardDir,
    crMvAbsolute,
    crMvBy,
    crMvByNoStride,
    crMvForward,
    creatureTurnTo,
) where

import Linear
import Control.Lens
import Dodge.Base
import Dodge.Creature.Statistics
import Dodge.Data.World
import Geometry

{- | Creature attempts to moves under its own steam.
The idea is that this may or may not work, depending on the status of the creature.
For now, though, this cannot fail.
-}
crMvBy ::
    -- | Movement translation vector, will be made relative to creature direction
    Point2 ->
    LWorld ->
    Creature ->
    Creature
crMvBy p lw cr = crMvAbsolute lw (rotateV (_crDir cr) p) cr

crMvByNoStride ::
    -- | Movement translation vector, will be made relative to creature direction
    Point2 ->
    LWorld ->
    Creature ->
    Creature
crMvByNoStride p lw cr = crMvAbsoluteNoStride lw (rotateV (_crDir cr) p) cr

crMvAbsolute :: LWorld -> Point2 -> Creature -> Creature
crMvAbsolute lw p' cr =
--    advanceStepCounter (magV p) 
      cr
        & crPos . _xy +~ p
        & crMvDir .~ argV p
  where
    p = strengthFactor (getCrMoveSpeed lw cr) *.* p'

crMvAbsoluteNoStride :: LWorld -> Point2 -> Creature -> Creature
crMvAbsoluteNoStride lw p' cr = cr & crPos . _xy +~ p
  where
    p = strengthFactor (getCrMoveSpeed lw cr) *.* p'

strengthFactor :: Int -> Float
strengthFactor i
    | i > 50 = 1
    | i < 1 = 0
    | otherwise = 0.02 * fromIntegral i

crMvForward :: Float -> LWorld -> Creature -> Creature
crMvForward speed = crMvBy (V2 speed 0)

advanceStepCounter :: Float -> Creature -> Creature
advanceStepCounter speed cr = cr & crStance . carriage %~ f
  where
    f car = case car of
        Standing -> f (Walking 0 RightForward)
        Walking i ff -> Walking (min (cr ^. crStance . strideLength) (i + speed)) ff
        _ -> car

creatureTurnTo :: Point2 -> Creature -> Creature
creatureTurnTo p cr
    | vToTarg == V2 0 0 = cr -- this should deal with the angleVV error
    | otherwise = cr & crDir .~ dirToTarget
  where
    vToTarg = p -.- cr ^. crPos . _xy
    dirToTarget = argV vToTarg

-- the following is perhaps not ideal because it mixes normalizeAngle with
-- angleVV, but it seems to work
creatureTurnTowardDir :: Float -> Float -> Creature -> Creature
creatureTurnTowardDir a turnSpeed cr
    | abs (normalizeAnglePi (a - cdir)) <= turnSpeed = cr & crDir .~ dirToTarget
    | otherwise = cr & crDir %~ addOrSub turnSpeed
  where
    addOrSub
        | isLeftOfA dirToTarget cdir = (+)
        | otherwise = subtract
    cdir = _crDir cr
    dirToTarget = argV $ rotateV a (V2 1 0)

creatureTurnToward :: Point2 -> Float -> Creature -> Creature
creatureTurnToward p turnSpeed cr
    | vToTarg == V2 0 0 = cr -- this should deal with the angleVV error
--    | errorAngleVV 3 vToTarg (unitVectorAtAngle (_crDir cr)) <= turnSpeed =
    | angleVV vToTarg (unitVectorAtAngle (_crDir cr)) <= turnSpeed =
        cr & crDir .~ dirToTarget
    | isLeftOfA (normalizeAngle dirToTarget) (normalizeAngle $ _crDir cr) = cr & crDir +~ turnSpeed
    | otherwise = cr & crDir -~ turnSpeed
  where
    vToTarg = p -.- cr ^. crPos . _xy
    dirToTarget = argV vToTarg