Astar working. Heuristic function needs to be improved