Added Mashal's pathfinding code, couple optimizations in progress