Added fillEmpty heuristik and optimized Astar