Solved the pathfinding algorithm.
Solved and debugged pathfinding algorithm.