Recover BDSP interrupted legs through shared wall localization

SerialPrograms/Source/PokemonBDSP/PokemonBDSP_Tests.cpp

@@ -7,6 +7,7 @@
 #include "Common/Cpp/TestRunners/UnitTestDatabase.h"
 #include "Programs/AutoStory/PokemonBDSP_AutoStory_Actions_Tests.h"
 #include "Programs/AutoStory/PokemonBDSP_AutoStory_GridSim_Tests.h"
+#include "Programs/AutoStory/PokemonBDSP_AutoStory_RecoveryWalk_Tests.h"
 #include "Programs/AutoStory/PokemonBDSP_AutoStory_Maps_Tests.h"
 #include "Programs/AutoStory/PokemonBDSP_AutoStory_PlannedRoute_Tests.h"
 #include "Programs/AutoStory/PokemonBDSP_AutoStory_RoutePlanner_Tests.h"
@@ -37,6 +38,7 @@ void add_tests(UnitTestDatabase& database){
     add_tests_AutoStory_Walk(database);
     add_tests_AutoStory_PlannedRoute(database);
     add_tests_AutoStory_GridSim(database);
+    add_tests_AutoStory_RecoveryWalk(database);
     add_tests_AutoStory_StorySim(database);
 }
 

SerialPrograms/Source/PokemonBDSP/Programs/AutoStory/Data/PokemonBDSP_AutoStory_StoryMaps.cpp

@@ -41,7 +41,7 @@
 
 //  The tiles of `map` from `corner`, `width` x `height`, and the objects on
 //  them, as a map of its own. Its points are left out.
 MapData crop(const MapData& map, const TilePos& corner, int width, int height){
     MapData cropped;
     cropped.name = map.name;
     cropped.source = map.source;
@@ -54,10 +54,25 @@
     for (Pokemon::MapObject object : map.objects){
         object.x -= corner.x;
         object.y -= corner.y;
-        if (cropped.tiles.contains({object.x, object.y})){
+        if (object.wander){
+            object.wander->x0 -= corner.x;
+            object.wander->x1 -= corner.x;
+            object.wander->y0 -= corner.y;
+            object.wander->y1 -= corner.y;
+        }
+        if (cropped.tiles.contains({object.x, object.y}) || (object.wander
+            && object.wander->x1 >= 0 && object.wander->y1 >= 0
+            && object.wander->x0 < width && object.wander->y0 < height)){
             cropped.objects.push_back(std::move(object));
         }
     }
+    for (auto warp : map.warps){
+        warp.x -= corner.x;
+        warp.y -= corner.y;
+        if (cropped.tiles.contains({warp.x, warp.y})){
+            cropped.warps.push_back(std::move(warp));
+        }
+    }
     return cropped;
 }
 

SerialPrograms/Source/PokemonBDSP/Programs/AutoStory/Data/PokemonBDSP_AutoStory_StoryWorld.cpp

@@ -56,12 +56,17 @@ void StoryWorld::add(std::string name, const Pokemon::MapData& map, const TilePo
             throw InternalProgramError(nullptr, PA_CURRENT_FUNCTION, "Map placed twice: " + name);
         }
     }
-    Placed placed{std::move(name), map.tiles, {}, offset};
+    Placed placed{.name = std::move(name), .tiles = map.tiles, .occupied = {}, .wander = {}, .warps = {}, .offset = offset};
     for (const Pokemon::MapObject& object : map.objects){
         if (!object.wander){
             placed.occupied.push_back({object.x, object.y});
+        }else{
+            placed.wander.push_back(*object.wander);
         }
     }
+    for (const auto& warp : map.warps){
+        placed.warps.push_back({warp.x, warp.y});
+    }
     check_overlap(placed);
     m_maps.push_back(std::move(placed));
     rebuild();
@@ -138,6 +143,18 @@ void StoryWorld::paint(const Placed& placed, const TilePos& corner){
     for (const TilePos& pos : placed.occupied){
         Pokemon::cell_at(m_cells, {corner.x + pos.x, corner.y + pos.y}).occupant = PERSON;
     }
+    for (const auto& box : placed.wander){
+        for (int y = std::max(0, box.y0); y <= std::min(placed.tiles.height - 1, box.y1); y++){
+            for (int x = std::max(0, box.x0); x <= std::min(placed.tiles.width - 1, box.x1); x++){
+                Pokemon::cell_at(m_cells, {corner.x + x, corner.y + y}).recovery_hazard = true;
+            }
+        }
+    }
+    for (const auto& warp : placed.warps){
+        if (placed.tiles.contains(warp)){
+            Pokemon::cell_at(m_cells, {corner.x + warp.x, corner.y + warp.y}).recovery_hazard = true;
+        }
+    }
 }
 
 const StoryWorld::Cell* StoryWorld::cell(const TilePos& pos) const{
@@ -162,8 +179,11 @@ std::optional<TileKind> StoryWorld::tile(const TilePos& pos) const{
     return found->tile;
 }
 
-StoryWorld::Occupant StoryWorld::occupied(const TilePos& pos) const{
+StoryWorld::Occupant StoryWorld::occupied(const TilePos& pos, OccupancyUse use) const{
     const Cell* found = cell(pos);
+    if (found && use == OccupancyUse::WALL_RECOVERY && found->recovery_hazard){
+        return PERSON;
+    }
     return found == nullptr ? NOBODY : found->occupant;
 }
 

SerialPrograms/Source/PokemonBDSP/Programs/AutoStory/Data/PokemonBDSP_AutoStory_StoryWorld.h

@@ -53,7 +53,9 @@ class StoryWorld{
         PERSON,
         BLOCKED,
     };
-    Occupant occupied(const Pokemon::TilePos& pos) const;
+    //  Recovery also excludes wandering areas and declared warps.
+    enum class OccupancyUse{ MOVEMENT, WALL_RECOVERY };
+    Occupant occupied(const Pokemon::TilePos& pos, OccupancyUse use = OccupancyUse::MOVEMENT) const;
 
     //  Blocks a tile found in the way on the console. Tiles off the maps
     //  are left alone.
@@ -72,12 +74,15 @@ class StoryWorld{
         std::string name;
         Pokemon::Grid<Pokemon::TileKind> tiles;
         std::vector<Pokemon::TilePos> occupied;     //  local tiles
+        std::vector<Pokemon::TileBox> wander;       //  recovery hazards, not terrain
+        std::vector<Pokemon::TilePos> warps;
         Pokemon::TilePos offset;
     };
     struct Cell{
         bool covered = false;
         Pokemon::TileKind tile = Pokemon::TileKind::WALL;
         Occupant occupant = NOBODY;
+        bool recovery_hazard = false;
     };
 
     void check_overlap(const Placed& map) const;

SerialPrograms/Source/PokemonBDSP/Programs/AutoStory/Data/PokemonBDSP_AutoStory_WallRecovery.cpp

@@ -0,0 +1,155 @@
+/*  BDSP Wall Recovery
+ *
+ *  From: https://github.com/PokemonAutomation/
+ */
+#include <algorithm>
+#include "PokemonBDSP_AutoStory_WallRecovery.h"
+
+namespace PokemonAutomation{
+namespace NintendoSwitch{
+namespace PokemonBDSP{
+namespace{
+
+using Pokemon::Direction;
+using Pokemon::TilePos;
+
+//  A wall ray must stay on the same map and walk one tile per step. A
+//  ledge may be legal travel but must not become a localization anchor.
+std::optional<WallHit> safe_wall(const StoryWorld& world, TilePos from, Direction direction){
+    const auto map = world.to_local(from);
+    if (!map || !standable(world, from)
+        || world.occupied(from, StoryWorld::OccupancyUse::WALL_RECOVERY) == StoryWorld::PERSON){
+        return std::nullopt;
+    }
+    const WallHit wall = slide_to_wall(world, from, direction);
+    if (!wall.anchored
+        || world.occupied(wall.wall, StoryWorld::OccupancyUse::WALL_RECOVERY) == StoryWorld::PERSON){
+        return std::nullopt;
+    }
+    while (from != wall.stop){
+        const auto next = step(world, from, direction);
+        const auto local = next ? world.to_local(*next) : std::nullopt;
+        if (!next || *next != from.next(direction) || !local || local->map != map->map
+            || world.occupied(*next, StoryWorld::OccupancyUse::WALL_RECOVERY) == StoryWorld::PERSON){
+            return std::nullopt;
+        }
+        from = *next;
+    }
+    return wall;
+}
+
+class WallPolicy{
+public:
+    using Hypothesis = TilePos;
+    using Position = TilePos;
+    using Hold = TileLeg;
+
+    static Position position(const Hypothesis& at){ return at; }
+    static int tiles(const Hold& hold){ return (int)hold.tiles; }
+    static std::optional<Pokemon::WallRecoveryStep<Hypothesis, Hold>> hold(
+        const StoryWorld& world, const std::vector<Hypothesis>& from, Direction direction
+    ){
+        std::vector<Hypothesis> ends;
+        size_t longest = 0;
+        for (const auto& start : from){
+            const auto wall = safe_wall(world, start, direction);
+            if (!wall){
+                return std::nullopt;
+            }
+            longest = std::max(longest, wall->tiles);
+            if (std::find(ends.begin(), ends.end(), wall->stop) == ends.end()){
+                ends.push_back(wall->stop);
+            }
+        }
+        return Pokemon::WallRecoveryStep<Hypothesis, Hold>{
+            .positions = std::move(ends),
+            .hold = {.direction = direction, .tiles = longest, .ends_at_wall = true},
+        };
+    }
+
+};
+
+} // namespace
+
+std::vector<Pokemon::WallAnchor<Pokemon::TilePos, TileLeg>> wall_recovery_candidates(
+    const StoryWorld& world, const std::vector<Pokemon::TilePos>& from,
+    const Pokemon::WallRecoveryLimits& limits
+){
+    for (const auto& tile : from){
+        if (!standable(world, tile)){
+            return {};
+        }
+    }
+    return Pokemon::find_wall_anchors<WallPolicy>(from, world, limits);
+}
+
+std::vector<TilePos> interrupted_leg_positions(
+    const StoryWorld& world, const std::vector<TilePos>& from, Direction direction, size_t tiles
+){
+    std::vector<TilePos> positions;
+    for (TilePos at : from){
+        for (size_t count = 0; count <= tiles; count++){
+            if (std::find(positions.begin(), positions.end(), at) == positions.end()){
+                positions.push_back(at);
+            }
+            const auto next = step(world, at, direction);
+            if (count == tiles || !next){
+                break;
+            }
+            at = *next;
+        }
+    }
+    return positions;
+}
+
+namespace{
+std::vector<PlannedLeg> recovery_legs(
+    const StoryWorld& world, std::vector<TilePos> from, const std::vector<TileLeg>& holds
+){
+    std::vector<PlannedLeg> legs;
+    for (const auto& hold : holds){
+        std::vector<TilePos> after;
+        for (const auto& at : from){
+            const TilePos end = slide_to_wall(world, at, hold.direction).stop;
+            if (std::find(after.begin(), after.end(), end) == after.end()){
+                after.push_back(end);
+            }
+        }
+        legs.push_back({.leg = hold, .after = after});
+        from = std::move(after);
+    }
+    return legs;
+}
+} // namespace
+
+std::optional<std::vector<PlannedLeg>> plan_recovered_route(
+    const StoryWorld& world, const std::vector<TilePos>& from,
+    const std::function<bool(const TilePos&)>& goal, const RobustPlanCosts& costs
+){
+    if (from.empty()){
+        return std::nullopt;
+    }
+    if (std::all_of(from.begin(), from.end(), goal)){
+        return std::vector<PlannedLeg>();
+    }
+    if (from.size() == 1){
+        return plan_robust_route(world, from, goal, costs);
+    }
+    auto anchors = wall_recovery_candidates(world, from);
+    std::stable_sort(anchors.begin(), anchors.end(), [&](const auto& a, const auto& b){
+        return goal(a.at) && !goal(b.at);
+    });
+    for (const auto& anchor : anchors){
+        const auto back = plan_robust_route(world, std::vector<TilePos>{anchor.at}, goal, costs);
+        if (back){
+            auto route = recovery_legs(world, from, anchor.holds);
+            route.insert(route.end(), back->begin(), back->end());
+            return route;
+        }
+    }
+    return std::nullopt;
+}
+
+} // namespace PokemonBDSP
+} // namespace NintendoSwitch
+} // namespace PokemonAutomation

SerialPrograms/Source/PokemonBDSP/Programs/AutoStory/Data/PokemonBDSP_AutoStory_WallRecovery.h

@@ -0,0 +1,44 @@
+/*  BDSP Wall Recovery
+ *
+ *  From: https://github.com/PokemonAutomation/
+ *
+ *  BDSP's movement adapter for shared native wall convergence. Predictions
+ *  do not confirm controller execution; the walker must establish arrival.
+ */
+#ifndef PokemonAutomation_PokemonBDSP_AutoStory_WallRecovery_H
+#define PokemonAutomation_PokemonBDSP_AutoStory_WallRecovery_H
+
+#include "Pokemon/Navigation/Pokemon_WallRecovery.h"
+#include "PokemonBDSP_AutoStory_RoutePlanner.h"
+
+namespace PokemonAutomation{
+namespace NintendoSwitch{
+namespace PokemonBDSP{
+
+//  Safe holds for every possible position, within one explicitly placed map.
+//  Never anchors against a person, crosses a door/map edge or hops a ledge.
+//  Caller must uncover trainer sight/scene hazards absent from StoryWorld.
+std::vector<Pokemon::WallAnchor<Pokemon::TilePos, TileLeg>> wall_recovery_candidates(
+    const StoryWorld& world, const std::vector<Pokemon::TilePos>& from,
+    const Pokemon::WallRecoveryLimits& limits = Pokemon::WallRecoveryLimits()
+);
+
+//  Every legal prefix, including zero movement, of an interrupted counted leg.
+std::vector<Pokemon::TilePos> interrupted_leg_positions(
+    const StoryWorld& world, const std::vector<Pokemon::TilePos>& from,
+    Pokemon::Direction direction, size_t tiles
+);
+
+//  Establish a common wall anchor before continuing from an uncertain pose.
+//  Single positions retain the existing planner. Ambiguous positions without
+//  a safe anchor fail instead of choosing one. Goals already held need no walk.
+std::optional<std::vector<PlannedLeg>> plan_recovered_route(
+    const StoryWorld& world, const std::vector<Pokemon::TilePos>& from,
+    const std::function<bool(const Pokemon::TilePos&)>& goal,
+    const RobustPlanCosts& costs
+);
+
+} // namespace PokemonBDSP
+} // namespace NintendoSwitch
+} // namespace PokemonAutomation
+#endif