Almost-completed shortest path AI

Warping is a bit weird right now, going to be fixed