Added expansion tracking and better A-Star heuristic

The A-Star heuristic only worked if the people started on the right side. It now
works on the left as well.