private StorageState CalcStartState()
{
StorageState result = new StorageState(_nodes);
foreach (var n in _nodes)
{
result.State[n.NodeName] = n.StartSpaceUsed;
}
result.OpenNodeWayPoints.Enqueue(new Tuple <int, int>(_blockerLeft - 1, _blockerTop - 1));
result.OpenNodeWayPoints.Enqueue(new Tuple <int, int>(_blockerLeft / 2, 0));
result.DesiredDataOnX = MaxX();
result.DesiredDataOnY = 0;
return(result);
}
private int SolveCostEstimate(StorageState startState)
{
// Number of spots that an open node is away from the current desired state
int openDistance = 200;
int openGoalX, openGoalY = 0;
bool isGoalNode;
int depth;
Tuple <int, int> moveOpenTo = startState.CurrentOpenNodeWayPoint(out isGoalNode, out depth);
openGoalX = moveOpenTo.Item1;
openGoalY = moveOpenTo.Item2;
for (int x = 0; x < MaxX(); x++)
{
for (int y = 0; y < MaxY(); y++)
{
StorageNode checkNode = FindNode(x, y);
StorageNode sourceNode = FindNode(openGoalX, openGoalY);
if (checkNode.SpaceAvailable(startState.State) > sourceNode.SpaceUsed(startState.State) &&
sourceNode.SpaceAvailable(startState.State) > checkNode.SpaceUsed(startState.State))
{
int candidateOpenDistance = Math.Abs(openGoalX - x) + (Math.Abs(openGoalY - y));
if (!isGoalNode)
{
candidateOpenDistance += (50 * depth);
}
if (candidateOpenDistance < openDistance)
{
openDistance = candidateOpenDistance;
if (openDistance == (50 * depth))
{
startState.HitWayPoint();
}
}
}
}
}
// number of spots that the desired state is away from the root note
int distanceFromRoot = startState.DesiredDataOnX + startState.DesiredDataOnY;
_lastDistanceFromOpen = openDistance;
_lastDistanceFromRoot = distanceFromRoot;
// score increases exponentialy with distance
return((openDistance * openDistance * 10) + (distanceFromRoot * distanceFromRoot));
}
private int reconstruct_move_depth(Dictionary <string, StorageState> cameFrom, StorageState current)
{
Console.WriteLine();
int result = 0;
StorageState tempCurrent = current;
// PrintState(tempCurrent);
while (cameFrom.ContainsKey(tempCurrent.HashState()))
{
Console.WriteLine();
tempCurrent = cameFrom[tempCurrent.HashState()];
// PrintState(tempCurrent);
result++;
}
return(result);
}
private void CalcAllPossibleValidMoves(StorageState current, ConcurrentQueue <StorageState> neighbours)
{
for (int x = 0; x <= MaxX(); x++)
{
for (int y = 0; y <= MaxY(); y++)
{
StorageNode node = FindNode(x, y);
// Can we move the node's data left?
CheckNodeMove(current, node, node.Left, neighbours);
// Can we move the node's data right?
CheckNodeMove(current, node, node.Right, neighbours);
// Can we move the node's data up?
CheckNodeMove(current, node, node.Top, neighbours);
// Can we move the node's data down?
CheckNodeMove(current, node, node.Bottom, neighbours);
}
}
}
private void PrintState(StorageState tempCurrent)
{
for (int y = 0; y <= MaxY(); y++)
{
StringBuilder line = new StringBuilder();
for (int x = 0; x <= MaxX(); x++)
{
StorageNode node = FindNode(x, y);
string display = "";
display = node.SpaceUsed(tempCurrent.State).ToString() + "/" +
node.SpaceAvailable(tempCurrent.State).ToString();
if (tempCurrent.DesiredDataOnX == x && tempCurrent.DesiredDataOnY == y)
{
display += "(G)";
}
line.Append(display.PadRight(6));
}
Console.WriteLine(line.ToString());
}
}
public int ShortestPath()
{
int consoleY = Console.CursorTop;
StorageState startState = CalcStartState();
// The set of moves already evaluated.
HashSet <string> closedSet = new HashSet <string>();
// The set of currently discovered nodes still to be evaluated.
// Initially, only the start node is known.
List <StorageState> openSet = new List <StorageState>();
openSet.Add(startState);
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the
// most efficient previous step.
Dictionary <string, StorageState> cameFrom = new Dictionary <string, StorageState>();
// For each node, the cost of getting from the start node to that node.
Dictionary <string, int> gScore = new Dictionary <string, int>();
// The cost of going from start to start is zero.
gScore[startState.HashState()] = 0;
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
Dictionary <string, int> fScore = new Dictionary <string, int>();
// For the first node, that value is completely heuristic.
fScore[startState.HashState()] = SolveCostEstimate(startState);
_lastDistanceFromOpen = 0;
_lastDistanceFromRoot = 0;
while (openSet.Count > 0)
{
Console.SetCursorPosition(0, consoleY);
Console.WriteLine("Queue depth: " + openSet.Count + " Hashes processed: " + closedSet.Count +
" Last Open Distance: " + _lastDistanceFromOpen.ToString() + " Last Distance from Solved: " +
_lastDistanceFromRoot.ToString());
var v = from f in openSet
join j in fScore
on f.HashState() equals j.Key
select new { score = j.Value, storageState = f };
var scores = v.ToList();
int minValue = scores.Min(o => o.score);
// the node in openSet having the lowest fScore[] value
StorageState current = scores.First(o => o.score == minValue).storageState;
//Console.WriteLine("Chose this state as the best one: " + current.ID);
//PrintState(current);
//Console.ReadLine();
if (current.DesiredDataOnX == 0 && current.DesiredDataOnY == 0)
{
return(reconstruct_move_depth(cameFrom, current));
}
openSet.Remove(current);
closedSet.Add(current.HashState());
ConcurrentQueue <StorageState> neighbours = new ConcurrentQueue <StorageState>();
//int moveCount = 0;
CalcAllPossibleValidMoves(current, neighbours);
foreach (StorageState bm in neighbours)
{
//Console.WriteLine("Possible move #" + (++moveCount).ToString() + " id: " + bm.ID);
//PrintState(bm);
// Ignore the neighbor which is already evaluated.
if (closedSet.Contains(bm.HashState()))
{
continue;
}
string hash = current.HashState();
// The distance from start to a neighbor
int tentative_gScore = gScore[hash] + 1;
// Discover a new node
if (!openSet.Contains(bm))
{
openSet.Add(bm);
}
else if (tentative_gScore >= gScore[bm.HashState()])
{
continue; // This is not a better path.
}
// This path is the best until now. Record it!
cameFrom[bm.HashState()] = current;
gScore[bm.HashState()] = tentative_gScore;
fScore[bm.HashState()] = gScore[bm.HashState()] + SolveCostEstimate(bm);
}
}
return(-1);
}
