static void Main(string[] args)
{
bool isDebug = true;
if (isDebug == false)
{
List <WorldState> initialStates = ServerCommunicator.readLevel();
List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> hMatrix = BFSHeruistic.PrecalcH(initialStates);
var testCBS = new CBS(hMatrix);
var testSolution = testCBS.findSolution();
List <string> lines = ServerCommunicator.printSolutionToFile(testSolution);
int counter = 0;
foreach (string line in lines)
{
Console.WriteLine(line);
string result = Console.ReadLine();
counter++;
if (!result.Contains("false"))
{
continue;
}
else
{
throw new Exception(line + result + "counter = " + counter);
}
}
}
else
{
var initialStates = ServerCommunicator.readLevelFromFile();
List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> hMatrix = BFSHeruistic.PrecalcH(initialStates);
var testCBS = new CBS(hMatrix);
var testSolution = testCBS.findSolution();
throw new Exception("at end");
}
/*
* Box box1 = new Box('a', "red");
* Box box2 = new Box('a', "red");
*
* Location location1 = new Location(3, 3);
* Location location2 = new Location(3, 3);
*
* var hash1 = HashCode.Combine(box1, location1);
* var hash2 = HashCode.Combine(box2, location2);
*/
//var test = ServerCommunicator.readLevel();
/// SINGLE AGENT TESTING//
/*
* Console.WriteLine("TESTAI");
* var initialStates = ServerCommunicator.readLevelFromFile();
* if (initialStates.Count > 1) throw new Exception();
*
* var watch1 = System.Diagnostics.Stopwatch.StartNew();
* //List<WorldState> plan1 = initialStates[0].agent.SearchClient.MakePlan(initialStates[0], new HashSet<Constraint>());
* watch1.Stop();
* var elapsedMs1 = watch1.ElapsedMilliseconds;
*
* var watch2 = System.Diagnostics.Stopwatch.StartNew();
* List<WorldState> plan2 = Astar.MakePlan(initialStates[0].agent, initialStates[0], new HashSet<Constraint>());
* watch2.Stop();
* var elapsedMs2 = watch2.ElapsedMilliseconds;
*
* List<List<WorldState>> solution = new List<List<WorldState>>();
* solution.Add(plan2);
* List<string> lines = ServerCommunicator.printSolutionToFile(solution);
*
* foreach (string line in lines)
* {
* Console.WriteLine(line);
* string result = Console.ReadLine();
*
* if (!result.Contains("false")) continue;
* else throw new Exception(line + result);
* }
*
* throw new Exception("at end");
*
*/
//////////////////////
/*
* bool runDebug = true;
* if (runDebug)
* {
* var test = ServerCommunicator.readLevelFromFile();
*
* var testCBS = new CBS(test);
*
* var testSolution = testCBS.findSolution();
* List<string> lines = ServerCommunicator.printSolutionToFile(testSolution);
*
*
* throw new Exception("We are at the end");
* }
* else
* {
* var test = ServerCommunicator.readLevel();
* var testCBS = new CBS(test);
* var testSolution = testCBS.findSolution();
* List<string> lines = ServerCommunicator.printSolutionToFile(testSolution);
*
* foreach (string line in lines)
* {
* Console.WriteLine(line);
* string result = Console.ReadLine();
*
* if (!result.Contains("false")) continue;
* else throw new Exception(line + result);
* }
* }*/
}
private static List <WorldState> AssignBoxesToAgents(List <EntityLocation> agents, List <EntityLocation> boxes, List <EntityLocation> agentGoals, List <EntityLocation> boxGoals, HashSet <Location> walls)
{
List <WorldState> initialStates = new List <WorldState>();
List <Assignment> boxAssignments = new List <Assignment>();
List <Assignment> goalAssignments = new List <Assignment>();
bool isAgentGoal = false;
List <WorldState> completeInitStates = new List <WorldState>();
foreach (EntityLocation agentEntity in agents)
{
WorldState completeInitState = new WorldState();
completeInitState.agent = (Agent)agentEntity.Entity;
completeInitState.agentLocation = agentEntity.Location;
completeInitState.Walls = walls;
completeInitState.assignedBoxes = new Dictionary <Location, Box>();
completeInitState.boxGoals = new List <EntityLocation>();
completeInitState.boxGoals.Add(new EntityLocation(completeInitState.agent, completeInitState.agentLocation));
completeInitStates.Add(completeInitState);
}
List <(WorldState, List <(EntityLocation, int[, ], float priority)>)> completeInitStatesWithH = BFSHeruistic.PrecalcH(completeInitStates);
while (boxGoals.Count > 0)
{
foreach (EntityLocation agentEntity in agents)
{
double shortestDistance = double.PositiveInfinity;
EntityLocation bestBoxGoalLocation = null;
EntityLocation bestInitialBoxGoalLocation = null;
Agent agent = (Agent)agentEntity.Entity;
EntityLocation agentGoal = null;
if (agentGoals.Count > 0)
{
isAgentGoal = agentGoals.Any(a => a.Entity.Name.Equals(agentEntity.Entity.Name));
if (isAgentGoal)
{
agentGoal = agentGoals.First(a => a.Entity.Name.Equals(agentEntity.Entity.Name));
}
}
foreach (EntityLocation boxGoalLocation in boxGoals.Where(b => b.Entity.Colour.Equals(agent.Colour)))
{
// Assign a goal and a box
foreach (EntityLocation boxInitialLocation in boxes.Where(b => b.Entity.Name.Equals(boxGoalLocation.Entity.Name)))
{
double total_dist = 0.0;
total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First()
.Item2[boxInitialLocation.Location.x, boxInitialLocation.Location.y]; //agentEntity.ManhattanDistance(boxInitialLocation);
total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First()
.Item2[boxGoalLocation.Location.x, boxGoalLocation.Location.y]; //boxInitialLocation.ManhattanDistance(boxGoalLocation);
if (isAgentGoal)
{
total_dist += completeInitStatesWithH.First(c => c.Item1.agent.Name.Equals(agentEntity.Entity.Name)).Item2.First()
.Item2[agentGoal.Location.x, agentGoal.Location.y]; //boxGoalLocation.ManhattanDistance(agentGoal);
}
if (total_dist < shortestDistance)
{
shortestDistance = total_dist;
bestBoxGoalLocation = boxGoalLocation;
bestInitialBoxGoalLocation = boxInitialLocation;
}
}
}
if (bestBoxGoalLocation != null && bestInitialBoxGoalLocation != null && shortestDistance < int.MaxValue)
{
boxAssignments.Add(new Assignment(agent, bestInitialBoxGoalLocation));
goalAssignments.Add(new Assignment(agent, bestBoxGoalLocation));
EntityLocation boxToRemove = null;
EntityLocation boxGoalToRemove = null;
foreach (EntityLocation b in boxes)
{
if (b.Location.Equals(bestInitialBoxGoalLocation.Location))
{
boxToRemove = b;
}
}
foreach (EntityLocation b in boxGoals)
{
if (b.Location.Equals(bestBoxGoalLocation.Location))
{
boxGoalToRemove = b;
}
}
boxes.Remove(boxToRemove);
boxGoals.Remove(boxGoalToRemove);
}
}
}
foreach (EntityLocation b in boxes)
{
int minBoxCount = int.MaxValue;
Agent minAgent = null;
foreach (EntityLocation agent in agents.Where(ass => ass.Entity.Colour.Equals(b.Entity.Colour)))
{
int thisAgentCount = 0;
foreach (Assignment a in boxAssignments.Where(ass => ass.Agent.Colour.Equals(b.Entity.Colour)))
{
if (a.Agent.Equals(agent.Entity))
{
thisAgentCount++;
}
}
if (thisAgentCount < minBoxCount)
{
minBoxCount = thisAgentCount;
minAgent = (Agent)agent.Entity;
}
}
if (minAgent == null)
{
throw new Exception("");
}
boxAssignments.Add(new Assignment(minAgent, b));
}
foreach (EntityLocation agentEntity in agents)
{
Location agentLocation = agentEntity.Location;
Location agentGoal;
List <EntityLocation> test = agentGoals.Where(a => a.Entity.Name.Equals(agentEntity.Entity.Name)).ToList();
if (test.Count > 0)
{
agentGoal = test.ElementAt(0).Location;
}
else
{
agentGoal = null;
}
Dictionary <Location, Box> agentBoxes = new Dictionary <Location, Box>();
List <EntityLocation> boxGoalsforThisAgent = new List <EntityLocation>();
foreach (Assignment boxAssignment in boxAssignments)
{
if (boxAssignment.Agent.Name.Equals(agentEntity.Entity.Name))
{
agentBoxes.Add(boxAssignment.EntityLocation.Location, (Box)boxAssignment.EntityLocation.Entity);
}
}
foreach (Assignment goalAssignment in goalAssignments)
{
if (goalAssignment.Agent.Name.Equals(agentEntity.Entity.Name))
{
boxGoalsforThisAgent.Add(goalAssignment.EntityLocation);
}
}
initialStates.Add(new WorldState(agentLocation, agentGoal, agentBoxes, boxGoalsforThisAgent, walls, (Agent)agentEntity.Entity));
}
return(initialStates);
}
