Basic A* algorithm

Crashes sometimes