Most recent astar search working (weighted a*)