Working pathfinding - needs optimisation and a complete rewrite, but other than that, not bad