Started breaking code into smaller pieces and moving into functions in
AStar.cpp