Basic algorithm with output
Completion of 2d pathfinding algorithm and output.