Pathfinding complete... ish

Need to check waypoints and interpolation.