Added optimizations and astar works but only on small cases