private (bool isReachable, Box closestBox) CheckIfGoalReachable(Goal goal, HashSet <Goal> otherUnprioritizedGoals, HashSet <Box> matchedBoxesSet)
{
var visited = new bool[level.Rows, level.Columns];
visited[goal.Position.X, goal.Position.Y] = true;
var Q = new Queue <Position>();
Q.Enqueue(goal.Position);
while (Q.Any())
{
var position = Q.Dequeue();
foreach (var direction in ActionGenerator.AllDirections)
{
var boxPosition = ActionGenerator.GeneratePositionFromDirection(position, direction);
// Check if in bounds
if (boxPosition.X < 0 || boxPosition.X >= level.Rows || boxPosition.Y < 0 || boxPosition.Y >= level.Columns)
{
continue;
}
if (!visited[boxPosition.X, boxPosition.Y])
{
var @object = level.Map[boxPosition.X, boxPosition.Y];
var goalObject = level.GoalsMap[boxPosition.X, boxPosition.Y];
// Return if the goal is reachable
if (@object is Box box && box.Symbol == goal.Symbol && !matchedBoxesSet.Contains(box))
{
return(true, box);
}
// what if @object is an Agent?
if (@object is Wall)
{
continue;
}
if (goalObject is Goal potentialUnprioritizedGoal &&
otherUnprioritizedGoals.Contains(potentialUnprioritizedGoal))
{
continue;
}
visited[boxPosition.X, boxPosition.Y] = true;
Q.Enqueue(boxPosition);
}
}
}
return(false, null);
}
/// <summary>
/// Calculates distances from all positions to all goals
/// </summary>
private void CalculateDistancesToGoals()
{
DistancesToGoal = new Dictionary <Goal, int[, ]>();
foreach (var goal in Level.AllGoals)
{
// Omit if there aren't any boxes for the goal
if (!Level.Boxes.Any(box => box.Symbol == goal.Symbol))
{
continue;
}
var distances = new int[Level.Rows, Level.Columns];
for (var i = 0; i < Level.Rows; i++)
{
for (var j = 0; j < Level.Columns; j++)
{
distances[i, j] = int.MaxValue;
}
}
distances[goal.Position.X, goal.Position.Y] = 0;
var Q = new Queue <Position>();
Q.Enqueue(goal.Position);
while (Q.Any())
{
var position = Q.Dequeue();
foreach (var direction in ActionGenerator.AllDirections)
{
var boxPosition = ActionGenerator.GeneratePositionFromDirection(position, direction);
// check if in bounds
if (boxPosition.X < 0 || boxPosition.X >= Level.Rows || boxPosition.Y < 0 || boxPosition.Y >= Level.Columns)
{
continue;
}
if (distances[boxPosition.X, boxPosition.Y] == int.MaxValue)
{
if (!(Level.Map[boxPosition.X, boxPosition.Y] is Wall))
{
distances[boxPosition.X, boxPosition.Y] = distances[position.X, position.Y] + 1;
Q.Enqueue(boxPosition);
}
}
}
}
DistancesToGoal[goal] = distances;
}
}
public override List <List <Action> > Solve()
{
// Thoughts:
// Goal pull distance doesn't work well when there are a lot of boxes.
var actions = new List <Action>();
var(found, goalsPrioritized, goalsUnprioritized) = CalculatePrioritizedGoals(level.AllGoals);
if (!found)
{
var(lockingGoals, otherGoalsPrioritized) = FindLockingGoals(goalsUnprioritized);
// TODO: What to do with locking goals?
// Put the locking box as far as possible from both prioritized goals and their boxes?
// Look at SARiddle.lvl and SALocking2.lvl
throw new System.NotImplementedException("Whoops, look at those locking goals ðŸ¤");
}
var currentState = new State(level.Map, level.AgentsPositions, level.BoxesPositions, level.GoalsForBox, level.AllGoals);
State lastSolutionState = null;
frontierSet = new HashSet <State>(new StateEqualityComparer());
foreach (var goal in goalsPrioritized)
{
// 1. Get to the target box.
// 2. Get to the target goal.
// 3. Continue.
var heuristic = new AStar(level, new PrioritizeHeuristic(goal));
var exploredStates = new HashSet <State>(new StateEqualityComparer());
if (frontier == null)
{
frontier = new PriorityQueue <State>(new StateHeuristicComparer(heuristic));
AddToFrontier(currentState);
}
else
{
frontier.SetComparer(heuristic);
}
var solutionFound = false;
while (!solutionFound)
{
// If frontier is empty, then no solution was found
if (frontier.IsEmpty())
{
break;
}
// Get the next state
currentState = RemoveFromFrontier();
// Check if we reached the box
if (goal.IsBoxGoal)
{
var boxFound = false;
// Is the box within any direction from the agent?
foreach (var direction in ActionGenerator.AllDirections)
{
var possibleBoxPosition = ActionGenerator.GeneratePositionFromDirection(currentState.AgentPosition, direction);
var goalPosition = currentState.BoxesPositions[goal.BoxForGoal];
if (goalPosition.Equals(possibleBoxPosition))
{
boxFound = true;
break;
}
}
if (boxFound)
{
solutionFound = true;
lastSolutionState = currentState;
}
}
// Check if we reached the goal
else if (currentState.Map[goal.Position.X, goal.Position.Y] is Box box && box.Symbol == goal.Symbol)
{
solutionFound = true;
lastSolutionState = currentState;
}
// Mark state as visited
exploredStates.Add(currentState);
// If solution for the current goal found
if (solutionFound)
{
frontier.Clear();
frontierSet.Clear();
// lastSolutionState.Print();
if (!goal.IsBoxGoal)
{
var stateWithFixedGoal = lastSolutionState.WithFixedGoal(goal);
// stateWithFixedGoal.Print();
AddToFrontier(stateWithFixedGoal);
}
}
// Add achievable and unvisited nodes to frontier
currentState.PopulateAchievableStates();
foreach (var state in currentState.AchievableStates)
{
if (LeadsToBlock(state))
{
state.Penalty += 100000;
}
if (!exploredStates.Contains(state) && !frontierSet.Contains(state))
{
AddToFrontier(state);
}
}
}
}
return(new List <List <Action> >()
{
lastSolutionState.ActionsUsedToReachState
});
}
