Fix pathfinding

The previous implementation didn't behave quite like A-* is supposed to.

After trying to figure out what's causing it and giving up,
I've reimplemented it in a straightforward manner.

Now it seems to work a lot better.