Add coloring (visited map) to idastar