Fixed map finding problem. Algorithm works for 4x4

TODO:
Check if heuristic works correclty. I think there are too many nodes
generated for low number of moves needed to solve.