Fixed A* algm a bit. Herustics function needs improving.