Tiny code clean-ups before real work.
Implemented A* pathfinding algorithm, but definitely requires more optimization