public void Execute()
        {
            // Resize containers
            int graphSize = DepthMap.Count();

            SelectedStatesByHorizon.Capacity = math.max(SelectedStatesByHorizon.Capacity, MaxDepth + 1);
            PredecessorStates.Capacity       = math.max(PredecessorStates.Capacity, graphSize);
            HorizonStateList.Capacity        = math.max(HorizonStateList.Capacity, graphSize);

            var selectedStateKeys = SelectedStates.GetKeyArray(Allocator.Temp);

            for (int i = 0; i < selectedStateKeys.Length; i++)
            {
                var stateKey   = selectedStateKeys[i];
                int stateDepth = int.MinValue;
                SelectedStates.TryGetFirstValue(stateKey, out var selectedDepth, out var iterator);
                do
                {
                    stateDepth = math.max(stateDepth, selectedDepth);
                }while (SelectedStates.TryGetNextValue(out selectedDepth, ref iterator));

                // Update depth map
                DepthMap[stateKey] = stateDepth;
                SelectedStatesByHorizon.AddValueIfUnique(stateDepth, stateKey);
            }
        }
        public void Execute()
        {
            var statesToUpdateLength = SelectedStates.Count();
            var statesToUpdate       = new NativeMultiHashMap <int, TStateKey>(statesToUpdateLength, Allocator.Temp);
            var currentHorizon       = new NativeList <TStateKey>(statesToUpdateLength, Allocator.Temp);
            var nextHorizon          = new NativeList <TStateKey>(statesToUpdateLength, Allocator.Temp);

            var maxDepth          = int.MinValue;
            var selectedStateKeys = SelectedStates.GetKeyArray(Allocator.Temp);

            for (int i = 0; i < selectedStateKeys.Length; i++)
            {
                var stateKey   = selectedStateKeys[i];
                int stateDepth = int.MinValue;
                SelectedStates.TryGetFirstValue(stateKey, out var selectedDepth, out var iterator);
                do
                {
                    stateDepth = math.max(stateDepth, selectedDepth);
                }while (SelectedStates.TryGetNextValue(out selectedDepth, ref iterator));

                // Update depth map
                DepthMap[stateKey] = stateDepth;

                // Queue state and track max depth of backpropagation
                statesToUpdate.AddValueIfUnique(stateDepth, stateKey);
                maxDepth = math.max(maxDepth, stateDepth);
            }
            selectedStateKeys.Dispose();

            var actionLookup              = planGraph.ActionLookup;
            var resultingStateLookup      = planGraph.ResultingStateLookup;
            var actionInfoLookup          = planGraph.ActionInfoLookup;
            var stateInfoLookup           = planGraph.StateInfoLookup;
            var stateTransitionInfoLookup = planGraph.StateTransitionInfoLookup;
            var predecessorLookup         = planGraph.PredecessorGraph;
            var depth = maxDepth;

            // Pull states from statesToUpdate
            if (statesToUpdate.TryGetFirstValue(depth, out var stateToAdd, out var stateIterator))
            {
                do
                {
                    currentHorizon.AddIfUnique(stateToAdd);
                }while (statesToUpdate.TryGetNextValue(out stateToAdd, ref stateIterator));
            }

            // Update values from leaf state(s) to root
            while (depth >= 0)
            {
                for (int i = 0; i < currentHorizon.Length; i++)
                {
                    var stateKey    = currentHorizon[i];
                    var updateState = true;
                    if (actionLookup.TryGetFirstValue(stateKey, out var actionKey, out var stateActionIterator))
                    {
                        // Expanded state. Only update if one or more actions have updated.
                        updateState = false;

                        // Update all actions
                        do
                        {
                            updateState |= UpdateCumulativeReward(new StateActionPair <TStateKey, TActionKey>(stateKey, actionKey), resultingStateLookup, stateInfoLookup, actionInfoLookup, stateTransitionInfoLookup);
                        }while (actionLookup.TryGetNextValue(out actionKey, ref stateActionIterator));
                    }

                    if (!updateState)
                    {
                        continue;
                    }

                    // Update state
                    if (UpdateStateValue(stateKey, actionLookup, stateInfoLookup, actionInfoLookup))
                    {
                        // If a change has occured, update predecessors
                        if (predecessorLookup.TryGetFirstValue(stateKey, out var predecessorStateKey, out var predecessorIterator))
                        {
                            do
                            {
                                nextHorizon.AddIfUnique(predecessorStateKey);
                            }while (predecessorLookup.TryGetNextValue(out predecessorStateKey, ref predecessorIterator));
                        }
                    }
                }

                var temp = currentHorizon;
                currentHorizon = nextHorizon;
                nextHorizon    = temp;
                nextHorizon.Clear();

                depth--;

                // pull out states from statesToUpdate
                if (statesToUpdate.TryGetFirstValue(depth, out stateToAdd, out stateIterator))
                {
                    do
                    {
                        currentHorizon.AddIfUnique(stateToAdd);
                    }while (statesToUpdate.TryGetNextValue(out stateToAdd, ref stateIterator));
                }
            }

            // new: continue propagating complete flag changes
            while (currentHorizon.Length > 0)
            {
                for (int i = 0; i < currentHorizon.Length; i++)
                {
                    var stateKey    = currentHorizon[i];
                    var updateState = false;

                    // Update all actions
                    actionLookup.TryGetFirstValue(stateKey, out var actionKey, out var stateActionIterator);
                    do
                    {
                        var stateActionPair = new StateActionPair <TStateKey, TActionKey>(stateKey, actionKey);
                        if (UpdateSubplanCompleteStatus(stateActionPair, out var updatedActionInfo))
                        {
                            updateState = true;

                            // Write back updated info
                            actionInfoLookup[stateActionPair] = updatedActionInfo;
                        }
                    }while (actionLookup.TryGetNextValue(out actionKey, ref stateActionIterator));

                    // Update state
                    if (updateState && UpdateSubplanCompleteStatus(stateKey, out var updatedStateInfo))
                    {
                        // Write back updated info
                        stateInfoLookup[stateKey] = updatedStateInfo;

                        // If a change has occured, update predecessors
                        if (predecessorLookup.TryGetFirstValue(stateKey, out var predecessorStateKey, out var predecessorIterator))
                        {
                            do
                            {
                                nextHorizon.AddIfUnique(predecessorStateKey);
                            }while (predecessorLookup.TryGetNextValue(out predecessorStateKey, ref predecessorIterator));
                        }
                    }
                }

                var temp = currentHorizon;
                currentHorizon = nextHorizon;
                nextHorizon    = temp;
                nextHorizon.Clear();
            }

            currentHorizon.Dispose();
            nextHorizon.Dispose();
            statesToUpdate.Dispose();
        }