Initial start of A* pathfinding algorithm. Under development.
Updated A* algorithm. Still not working. Also added test class.
Added some testing printouts.