A* search algorithm. Messy, and I'm not 100% sure it's correct.