-- |
-- SPDX-License-Identifier: BSD-3-Clause
--
-- Implementation of the 'Swarm.Language.Syntax.Path' command for robots.
--
-- = Design considerations
-- In general the playfield can be dynamic, and obstructions may
-- appear that invalidate a given computed shortest path.
-- Therefore, there would be limited value in a command that returns
-- an entirely static computed path that is somehow stored on the client side
-- (i.e. inside a swarm-lang program).
--
-- In the current implementation, a complete path is computed
-- internally upon invoking the @path@ command
-- and doled out incrementally across ticks.
-- Each @path@ invocation returns the direction of the
-- next "move" along the computed shortest path.
--
-- This internally stored path is re-used across invocations until some
-- event invalidates its cache (see "Swarm.Game.Step.Path.Cache").
--
-- == Max distance
--
-- We allow the caller to supply a max distance, but also impose an internal maximum
-- distance to prevent programming errors from irrecoverably freezing the game.
module Swarm.Game.Step.Path.Finding where

import Control.Carrier.State.Lazy
import Control.Effect.Lens
import Control.Lens ((^.))
import Control.Monad (filterM, guard)
import Control.Monad.Trans.Class (lift)
import Control.Monad.Trans.Maybe (MaybeT (..), runMaybeT)
import Data.Graph.AStar (aStarM)
import Data.HashSet (HashSet)
import Data.HashSet qualified as HashSet
import Data.Int (Int32)
import Data.List.NonEmpty (NonEmpty ((:|)))
import Swarm.Game.Entity
import Swarm.Game.Location
import Swarm.Game.Robot
import Swarm.Game.State
import Swarm.Game.Step.Path.Cache
import Swarm.Game.Step.Path.Cache.DistanceLimit (withinDistance)
import Swarm.Game.Step.Path.Type
import Swarm.Game.Step.RobotStepState
import Swarm.Game.Step.Util
import Swarm.Game.Step.Util.Inspect
import Swarm.Game.Universe
import Swarm.Language.Syntax
import Swarm.Language.Syntax.Direction
import Swarm.Util (hoistMaybe)

-- | Swarm command arguments are converted to idiomatic Haskell
-- types before invoking this function, and conversely the callsite
-- is also responsible for translating the output type to a swarm value.
--
-- The cost function is uniformly @1@ between adjacent cells.
--
-- Viable paths are determined by walkability.
-- If the goal type is an 'Entity', then it is permissible for that
-- entity to be 'Unwalkable'.
--
-- See "Swarm.Game.Step.Path.Cache" for caching details.
pathCommand ::
  HasRobotStepState sig m =>
  PathfindingParameters (Cosmic Location) ->
  m (Maybe (Direction, Int))
pathCommand :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
PathfindingParameters (Cosmic Location)
-> m (Maybe (Direction, Int))
pathCommand PathfindingParameters (Cosmic Location)
parms = do
  WalkabilityContext
currentWalkabilityContext <- Getting WalkabilityContext Robot WalkabilityContext
-> m WalkabilityContext
forall s a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State s) sig m =>
Getting a s a -> m a
use Getting WalkabilityContext Robot WalkabilityContext
Getter Robot WalkabilityContext
walkabilityContext

  -- First, check if the pathfinding target has a cached path.
  Either CacheRetreivalInapplicability [Location]
eitherCachedPath <- WalkabilityContext
-> PathfindingParameters (Cosmic Location)
-> m (Either CacheRetreivalInapplicability [Location])
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
WalkabilityContext
-> PathfindingParameters (Cosmic Location)
-> m (Either CacheRetreivalInapplicability [Location])
retrieveCachedPath WalkabilityContext
currentWalkabilityContext PathfindingParameters (Cosmic Location)
parms

  case Either CacheRetreivalInapplicability [Location]
eitherCachedPath of
    Right [Location]
foundCachedPath -> Maybe (Direction, Int) -> m (Maybe (Direction, Int))
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return (Maybe (Direction, Int) -> m (Maybe (Direction, Int)))
-> Maybe (Direction, Int) -> m (Maybe (Direction, Int))
forall a b. (a -> b) -> a -> b
$ (Direction, Int) -> Maybe (Direction, Int)
forall a. a -> Maybe a
Just ((Direction, Int) -> Maybe (Direction, Int))
-> (Direction, Int) -> Maybe (Direction, Int)
forall a b. (a -> b) -> a -> b
$ [Location] -> (Direction, Int)
mkResult [Location]
foundCachedPath
    Left CacheRetreivalInapplicability
_ -> do
      -- This is a short-circuiting optimization; if the goal location itself
      -- is not a walkable cell, then no amount of searching will reach it.
      Bool
isGoalLocWalkable <- case PathfindingTarget
target of
        LocationTarget Location
loc -> Maybe MoveFailureMode -> Bool
forall a. Maybe a -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null (Maybe MoveFailureMode -> Bool)
-> m (Maybe MoveFailureMode) -> m Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Cosmic Location -> m (Maybe MoveFailureMode)
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
Cosmic Location -> m (Maybe MoveFailureMode)
checkMoveFailure (SubworldName -> Location -> Cosmic Location
forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld Location
loc)
        EntityTarget EntityName
_ -> Bool -> m Bool
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return Bool
True

      MaybeT m (Direction, Int) -> m (Maybe (Direction, Int))
forall (m :: * -> *) a. MaybeT m a -> m (Maybe a)
runMaybeT (MaybeT m (Direction, Int) -> m (Maybe (Direction, Int)))
-> MaybeT m (Direction, Int) -> m (Maybe (Direction, Int))
forall a b. (a -> b) -> a -> b
$ do
        Bool -> MaybeT m ()
forall (f :: * -> *). Alternative f => Bool -> f ()
guard Bool
isGoalLocWalkable
        Maybe [Location]
maybeFoundPath <- m (Maybe [Location]) -> MaybeT m (Maybe [Location])
forall (m :: * -> *) a. Monad m => m a -> MaybeT m a
forall (t :: (* -> *) -> * -> *) (m :: * -> *) a.
(MonadTrans t, Monad m) =>
m a -> t m a
lift m (Maybe [Location])
computePath
        [Location]
foundPath <- Maybe [Location] -> MaybeT m [Location]
forall (m :: * -> *) b. Applicative m => Maybe b -> MaybeT m b
hoistMaybe Maybe [Location]
maybeFoundPath
        -- NOTE: This will not cache the fact that a path was not found.
        m () -> MaybeT m ()
forall (m :: * -> *) a. Monad m => m a -> MaybeT m a
forall (t :: (* -> *) -> * -> *) (m :: * -> *) a.
(MonadTrans t, Monad m) =>
m a -> t m a
lift (m () -> MaybeT m ()) -> m () -> MaybeT m ()
forall a b. (a -> b) -> a -> b
$ PathfindingParameters SubworldName
-> WalkabilityContext -> NonEmpty Location -> m ()
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
PathfindingParameters SubworldName
-> WalkabilityContext -> NonEmpty Location -> m ()
recordCache ((Cosmic Location -> SubworldName)
-> PathfindingParameters (Cosmic Location)
-> PathfindingParameters SubworldName
forall a b.
(a -> b) -> PathfindingParameters a -> PathfindingParameters b
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
fmap (Cosmic Location
-> Getting SubworldName (Cosmic Location) SubworldName
-> SubworldName
forall s a. s -> Getting a s a -> a
^. Getting SubworldName (Cosmic Location) SubworldName
forall a (f :: * -> *).
Functor f =>
(SubworldName -> f SubworldName) -> Cosmic a -> f (Cosmic a)
subworld) PathfindingParameters (Cosmic Location)
parms) WalkabilityContext
currentWalkabilityContext (NonEmpty Location -> m ()) -> NonEmpty Location -> m ()
forall a b. (a -> b) -> a -> b
$ Location
robotLoc Location -> [Location] -> NonEmpty Location
forall a. a -> [a] -> NonEmpty a
:| [Location]
foundPath
        (Direction, Int) -> MaybeT m (Direction, Int)
forall a. a -> MaybeT m a
forall (m :: * -> *) a. Monad m => a -> m a
return ((Direction, Int) -> MaybeT m (Direction, Int))
-> (Direction, Int) -> MaybeT m (Direction, Int)
forall a b. (a -> b) -> a -> b
$ [Location] -> (Direction, Int)
mkResult [Location]
foundPath
 where
  mkResult :: [Location] -> (Direction, Int)
mkResult [Location]
p = ([Location] -> Direction
nextDir [Location]
p, [Location] -> Int
forall a. [a] -> Int
forall (t :: * -> *) a. Foldable t => t a -> Int
length [Location]
p)
  PathfindingParameters Maybe Integer
maybeDistanceLimit (Cosmic SubworldName
currentSubworld Location
robotLoc) PathfindingTarget
target = PathfindingParameters (Cosmic Location)
parms

  computePath :: m (Maybe [Location])
computePath =
    (Location -> m (HashSet Location))
-> (Location -> Location -> m Int32)
-> (Location -> m Int32)
-> (Location -> m Bool)
-> m Location
-> m (Maybe [Location])
forall (m :: * -> *) a c.
(Monad m, Hashable a, Ord a, Ord c, Num c) =>
(a -> m (HashSet a))
-> (a -> a -> m c)
-> (a -> m c)
-> (a -> m Bool)
-> m a
-> m (Maybe [a])
aStarM
      ((Location -> Bool) -> Cosmic Location -> m (HashSet Location)
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
(Location -> Bool) -> Cosmic Location -> m (HashSet Location)
neighborFunc Location -> Bool
withinDistanceLimit (Cosmic Location -> m (HashSet Location))
-> (Location -> Cosmic Location)
-> Location
-> m (HashSet Location)
forall b c a. (b -> c) -> (a -> b) -> a -> c
. SubworldName -> Location -> Cosmic Location
forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld)
      ((Location -> m Int32) -> Location -> Location -> m Int32
forall a b. a -> b -> a
const ((Location -> m Int32) -> Location -> Location -> m Int32)
-> (Location -> m Int32) -> Location -> Location -> m Int32
forall a b. (a -> b) -> a -> b
$ m Int32 -> Location -> m Int32
forall a b. a -> b -> a
const (m Int32 -> Location -> m Int32) -> m Int32 -> Location -> m Int32
forall a b. (a -> b) -> a -> b
$ Int32 -> m Int32
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return Int32
1)
      (Int32 -> m Int32
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return (Int32 -> m Int32) -> (Location -> Int32) -> Location -> m Int32
forall b c a. (b -> c) -> (a -> b) -> a -> c
. Location -> Int32
distHeuristic)
      Location -> m Bool
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc
      (Location -> m Location
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return Location
robotLoc)

  withinDistanceLimit :: Location -> Bool
  withinDistanceLimit :: Location -> Bool
withinDistanceLimit = Integer -> Location -> Location -> Bool
withinDistance Integer
distLimit Location
robotLoc

  directionTo :: Location -> Direction
  directionTo :: Location -> Direction
directionTo Location
nextLoc = AbsoluteDir -> Direction
DAbsolute (AbsoluteDir -> Direction) -> AbsoluteDir -> Direction
forall a b. (a -> b) -> a -> b
$ V2 Int32 -> AbsoluteDir
nearestDirection (V2 Int32 -> AbsoluteDir) -> V2 Int32 -> AbsoluteDir
forall a b. (a -> b) -> a -> b
$ Location
nextLoc Location -> Location -> Diff (Point V2) Int32
forall a. Num a => Point V2 a -> Point V2 a -> Diff (Point V2) a
forall (p :: * -> *) a. (Affine p, Num a) => p a -> p a -> Diff p a
.-. Location
robotLoc

  -- Extracts the head of the found path to determine
  -- the next direction for the robot to proceed along
  nextDir :: [Location] -> Direction
  nextDir :: [Location] -> Direction
nextDir [Location]
pathLocs = case [Location]
pathLocs of
    [] -> RelativeDir -> Direction
DRelative RelativeDir
DDown
    (Location
nextLoc : [Location]
_) -> Location -> Direction
directionTo Location
nextLoc

  neighborFunc ::
    HasRobotStepState sig m =>
    (Location -> Bool) ->
    Cosmic Location ->
    m (HashSet Location)
  neighborFunc :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
(Location -> Bool) -> Cosmic Location -> m (HashSet Location)
neighborFunc Location -> Bool
isWithinRange Cosmic Location
loc = do
    [Cosmic Location]
locs <- (Cosmic Location -> m Bool)
-> [Cosmic Location] -> m [Cosmic Location]
forall (m :: * -> *) a.
Applicative m =>
(a -> m Bool) -> [a] -> m [a]
filterM Cosmic Location -> m Bool
forall {sig :: (* -> *) -> * -> *} {m :: * -> *}.
(Algebra sig m, Member (State Robot) sig,
 Member (State GameState) sig, Member (Throw Exn) sig) =>
Cosmic Location -> m Bool
isWalkableLoc [Cosmic Location]
neighborLocs
    HashSet Location -> m (HashSet Location)
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return (HashSet Location -> m (HashSet Location))
-> HashSet Location -> m (HashSet Location)
forall a b. (a -> b) -> a -> b
$ [Location] -> HashSet Location
forall a. (Eq a, Hashable a) => [a] -> HashSet a
HashSet.fromList ([Location] -> HashSet Location) -> [Location] -> HashSet Location
forall a b. (a -> b) -> a -> b
$ (Cosmic Location -> Location) -> [Cosmic Location] -> [Location]
forall a b. (a -> b) -> [a] -> [b]
map (Getting Location (Cosmic Location) Location
-> Cosmic Location -> Location
forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view Getting Location (Cosmic Location) Location
forall a1 a2 (f :: * -> *).
Functor f =>
(a1 -> f a2) -> Cosmic a1 -> f (Cosmic a2)
planar) [Cosmic Location]
locs
   where
    neighborLocs :: [Cosmic Location]
neighborLocs = Cosmic Location -> [Cosmic Location]
getNeighborLocs Cosmic Location
loc
    isWalkableLoc :: Cosmic Location -> m Bool
isWalkableLoc Cosmic Location
someLoc =
      if Bool -> Bool
not (Bool -> Bool) -> Bool -> Bool
forall a b. (a -> b) -> a -> b
$ Location -> Bool
isWithinRange (Location -> Bool) -> Location -> Bool
forall a b. (a -> b) -> a -> b
$ Getting Location (Cosmic Location) Location
-> Cosmic Location -> Location
forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view Getting Location (Cosmic Location) Location
forall a1 a2 (f :: * -> *).
Functor f =>
(a1 -> f a2) -> Cosmic a1 -> f (Cosmic a2)
planar Cosmic Location
someLoc
        then Bool -> m Bool
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return Bool
False
        else do
          Bool
isGoal <- Location -> m Bool
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc (Location -> m Bool) -> Location -> m Bool
forall a b. (a -> b) -> a -> b
$ Getting Location (Cosmic Location) Location
-> Cosmic Location -> Location
forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view Getting Location (Cosmic Location) Location
forall a1 a2 (f :: * -> *).
Functor f =>
(a1 -> f a2) -> Cosmic a1 -> f (Cosmic a2)
planar Cosmic Location
someLoc
          if Bool
isGoal
            then Bool -> m Bool
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return Bool
True
            else Maybe MoveFailureMode -> Bool
forall a. Maybe a -> Bool
forall (t :: * -> *) a. Foldable t => t a -> Bool
null (Maybe MoveFailureMode -> Bool)
-> m (Maybe MoveFailureMode) -> m Bool
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Cosmic Location -> m (Maybe MoveFailureMode)
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
HasRobotStepState sig m =>
Cosmic Location -> m (Maybe MoveFailureMode)
checkMoveFailureUnprivileged Cosmic Location
someLoc

  -- This is an optimization for when a specific location
  -- is given as the target.
  -- However, it is not strictly necessary, and in fact
  -- cannot be used when the target is a certain type of
  -- entity.
  distHeuristic :: Location -> Int32
  distHeuristic :: Location -> Int32
distHeuristic = case PathfindingTarget
target of
    LocationTarget Location
gLoc -> Location -> Location -> Int32
manhattan Location
gLoc
    EntityTarget EntityName
_eName -> Int32 -> Location -> Int32
forall a b. a -> b -> a
const Int32
0

  goalReachedFunc :: Has (State GameState) sig m => Location -> m Bool
  goalReachedFunc :: forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Location -> m Bool
goalReachedFunc Location
loc = case PathfindingTarget
target of
    LocationTarget Location
gLoc -> Bool -> m Bool
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return (Bool -> m Bool) -> Bool -> m Bool
forall a b. (a -> b) -> a -> b
$ Location
loc Location -> Location -> Bool
forall a. Eq a => a -> a -> Bool
== Location
gLoc
    EntityTarget EntityName
eName -> do
      Maybe Entity
me <- Cosmic Location -> m (Maybe Entity)
forall (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (State GameState) sig m =>
Cosmic Location -> m (Maybe Entity)
entityAt (Cosmic Location -> m (Maybe Entity))
-> Cosmic Location -> m (Maybe Entity)
forall a b. (a -> b) -> a -> b
$ SubworldName -> Location -> Cosmic Location
forall a. SubworldName -> a -> Cosmic a
Cosmic SubworldName
currentSubworld Location
loc
      Bool -> m Bool
forall a. a -> m a
forall (m :: * -> *) a. Monad m => a -> m a
return (Bool -> m Bool) -> Bool -> m Bool
forall a b. (a -> b) -> a -> b
$ (Getting EntityName Entity EntityName -> Entity -> EntityName
forall r a (sig :: (* -> *) -> * -> *) (m :: * -> *).
Has (Reader r) sig m =>
Getting a r a -> m a
view Getting EntityName Entity EntityName
Lens' Entity EntityName
entityName (Entity -> EntityName) -> Maybe Entity -> Maybe EntityName
forall (f :: * -> *) a b. Functor f => (a -> b) -> f a -> f b
<$> Maybe Entity
me) Maybe EntityName -> Maybe EntityName -> Bool
forall a. Eq a => a -> a -> Bool
== EntityName -> Maybe EntityName
forall a. a -> Maybe a
Just EntityName
eName

  -- A failsafe limit is hardcoded to prevent the game from freezing
  --  if an error exists in some .sw code.
  distLimit :: Integer
distLimit = Integer -> (Integer -> Integer) -> Maybe Integer -> Integer
forall b a. b -> (a -> b) -> Maybe a -> b
maybe Integer
maxPathRange (Integer -> Integer -> Integer
forall a. Ord a => a -> a -> a
min Integer
maxPathRange) Maybe Integer
maybeDistanceLimit