A_Star_MDDs.cs

using System;
using System.Collections.Generic;
using System.Linq;
using System.Diagnostics;
using ExtensionMethods;

namespace mapf
{
    /// <summary>
    /// Finds the solution with the least number of conflicts, given a set of MDDs
    /// </summary>
    class A_Star_MDDs : IConflictReporting
    {
        MDD[] problem;
        Dictionary<A_Star_MDDs_Node, A_Star_MDDs_Node> closedList;
        Run runner;
        BinaryHeap<A_Star_MDDs_Node> openList;
        public int expanded;
        public int generated;
        public int conflictCount;
        ConflictAvoidanceTable CAT;

        public A_Star_MDDs(MDD[] problem, Run runner, ConflictAvoidanceTable CAT)
        {
            this.expanded = 0;
            this.generated = 0;
            A_Star_MDDs_Node root;
            this.problem = problem;
            this.runner = runner;
            this.CAT = CAT;
            this.closedList = new Dictionary<A_Star_MDDs_Node, A_Star_MDDs_Node>();
            this.openList = new BinaryHeap<A_Star_MDDs_Node>();
            MDDNode[] sRoot = new MDDNode[problem.Length];
            for (int i = 0; i < problem.Length; i++)
            {
                sRoot[i] = problem[i].levels[0].First.Value;
            }
            root = new A_Star_MDDs_Node(sRoot, null);
            openList.Add(root);
            closedList.Add(root, root); // There will never be a hit. This is only done for consistancy
            conflictCount = 0;
        }

        public SinglePlan[] Solve()
        {
            A_Star_MDDs_Node currentNode;
            //A_Star_MDDs_Expander expander = new A_Star_MDDs_Expander();

            while (openList.Count > 0)
            {
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                {
                    return null;
                }
                currentNode = openList.Remove();
                // Check if node is the goal
                if (this.GoalTest(currentNode))
                {
                    this.conflictCount = currentNode.conflictCount;
                    this.conflictCounts = currentNode.conflictCounts;
                    this.conflictTimes = currentNode.conflictTimes;
                    return GetAnswer(currentNode);
                }

                // Expand
                expanded++;  // TODO: don't count re-expansions as expansions?
                Expand(currentNode);
                //expander.Setup(currentNode);
                //Expand(expander);  // TODO: the expander just generates all children. EPEA* its ass!!
            }
            return null;
        }

        public void Expand(A_Star_MDDs_Node node)
        {
            if (node.IsAlreadyExpanded() == false)
            {
                node.calcSingleAgentDeltaConflictCounts(this.CAT);
                node.alreadyExpanded = true;
                node.targetDeltaConflictCount = 0;
                node.remainingDeltaConflictCount = node.targetDeltaConflictCount; // Just for the following hasChildrenForCurrentDeltaConflictCount call.
                while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount() == false) // DeltaConflictCount==0 may not be possible if all agents have obstacles between their location and the goal
                {
                    node.targetDeltaConflictCount++;
                    node.remainingDeltaConflictCount = node.targetDeltaConflictCount;
                }
                if (node.hasMoreChildren() == false) // Node has no possible children at all
                {
                    node.ClearExpansionData();
                    return;
                }
            }

            var intermediateNodes = new List<A_Star_MDDs_Node>() { node };

            for (int mddIndex = 0; mddIndex < this.problem.Length && intermediateNodes.Count != 0; ++mddIndex)
            {
                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                    return;

                intermediateNodes = ExpandOneAgent(intermediateNodes, mddIndex);
            }

            var finalGeneratedNodes = intermediateNodes;

            foreach (var child in finalGeneratedNodes)
            {
                child.conflictCount = node.conflictCount + node.targetDeltaConflictCount;

                // Accumulating the conflicts count from parent to child
                // We're counting conflicts along the entire path, so the parent's conflicts count is added to the child's:
                child.conflictCounts = new Dictionary<int, int>(child.prev.conflictCounts);
                child.conflictTimes = new Dictionary<int, List<int>>();
                foreach (var kvp in child.prev.conflictTimes)
                    child.conflictTimes[kvp.Key] = new List<int>(kvp.Value);
                child.IncrementConflictCounts(this.CAT);  // We're counting conflicts along the entire path, so the parent's conflicts count
                                                          // is added to the child's.

                bool was_closed = this.closedList.ContainsKey(child);
                if (was_closed)
                {
                    A_Star_MDDs_Node inClosedList = this.closedList[child];

                    if (inClosedList.conflictCount > child.conflictCount)
                    {
                        closedList.Remove(inClosedList);
                        openList.Remove(inClosedList);
                        was_closed = false;
                    }
                }
                if (!was_closed)
                {
                    this.openList.Add(child);
                    this.closedList.Add(child, child);
                    generated++;
                }
            }

            // Prepare the node for the next partial expansion:
            if (node.IsAlreadyExpanded() == false)
            {
                // Node was cleared during expansion.
                // It's unnecessary and unsafe to continue to prepare it for the next partial expansion.
                return;
            }

            node.targetDeltaConflictCount++; // This delta F was exhausted
            node.remainingDeltaConflictCount = node.targetDeltaConflictCount;

            while (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount() == false)
            {
                node.targetDeltaConflictCount++;
                node.remainingDeltaConflictCount = node.targetDeltaConflictCount; // Just for the following hasChildrenForCurrentDeltaF call.
            }

            if (node.hasMoreChildren() && node.hasChildrenForCurrentDeltaConflictCount())
            {
                // Re-insert node into open list
                openList.Add(node);
            }
            else
                node.ClearExpansionData();
        }

        protected List<A_Star_MDDs_Node> ExpandOneAgent(List<A_Star_MDDs_Node> intermediateNodes, int mddIndex)
        {
            var generated = new List<A_Star_MDDs_Node>();

            // Expand the mdd node
            foreach (A_Star_MDDs_Node node in intermediateNodes)
            {
                // Try all the children of this MDD node
                foreach ((int childIndex, MDDNode childMddNode) in node.allSteps[mddIndex].children.Enumerate())
                {
                    if (node.currentMoves != null && childMddNode.move.IsColliding(node.currentMoves))  // Can happen. We only prune partially, we don't build the full k-agent MDD.
                        continue;

                    var childNode = new A_Star_MDDs_Node(node, mddIndex != node.allSteps.Length - 1);
                    childNode.allSteps[mddIndex] = childMddNode;

                    // Update target conflict count and prune nodes that can't get to the target conflict count
                    childNode.UpdateRemainingDeltaConflictCount(mddIndex, childIndex);
                    if (childNode.remainingDeltaConflictCount == ushort.MaxValue || // Last move was bad - not sure this can happen here
                        (childNode.hasChildrenForCurrentDeltaConflictCount(mddIndex + 1) == false))  // No children that can reach the target
                        continue;

                    if (mddIndex < node.allSteps.Length - 1) // More MDD nodes need to choose a child
                        childNode.currentMoves.Add(childMddNode.move);
                    else // Moved the last agent
                        childNode.currentMoves = null; // To reduce memory load and lookup times

                    // Set the node's prev to its real parent, skipping over the intermediate nodes.
                    if (mddIndex != 0)
                        childNode.prev = node.prev;
                    else
                        childNode.prev = node;

                    generated.Add(childNode);
                }
            }

            return generated;
        }

        protected Dictionary<int, int> conflictCounts;
        protected Dictionary<int, List<int>> conflictTimes;

        /// <summary>
        /// </summary>
        /// <returns>Map each external agent to the number of conflicts with their path the solution has</returns>
        public Dictionary<int, int> GetExternalConflictCounts()
        {
            return this.conflictCounts;
        }

        /// <summary>
        /// </summary>
        /// <returns>Map each external agent to a list of times the solution has a conflict with theirs</returns>
        public Dictionary<int, List<int>> GetConflictTimes()
        {
            return this.conflictTimes;
        }

        public void Expand(A_Star_MDDs_Expander currentNode)
        {
            while (true)
            {
                A_Star_MDDs_Node child = currentNode.GetNextChild();
                if (child == null)
                    break;

                if (IsLegalMove(child))
                {
                    child.conflictCount = child.prev.conflictCount;
                    child.UpdateConflicts(CAT);

                    bool was_closed = this.closedList.ContainsKey(child);
                    if (was_closed)
                    {
                        A_Star_MDDs_Node inClosedList = this.closedList[child];

                        if (inClosedList.conflictCount > child.conflictCount)
                        {
                            closedList.Remove(inClosedList);
                            openList.Remove(inClosedList);
                            was_closed = false;
                        }
                    }
                    if (!was_closed)
                    {
                        this.openList.Add(child);
                        this.closedList.Add(child, child);
                        generated++;
                    }
                }
            }
        }

        public int GetGenerated() { return this.generated; }

        public int GetExpanded() { return this.expanded; }

        private bool GoalTest(A_Star_MDDs_Node toCheck)
        {
            if (toCheck.GetDepth() == problem[0].levels.Length - 1)
                return true;
            return false;
        }

        private SinglePlan[] GetAnswer(A_Star_MDDs_Node finish)
        {
            // TODO: Move the construction of the SinglePlans to a static method in SinglePlan
            var routes = new LinkedList<Move>[problem.Length];
            for (int i = 0; i < routes.Length; i++)
                routes[i] = new LinkedList<Move>();

            A_Star_MDDs_Node current = finish;
            while (current != null)
            {
                for (int i = 0; i < problem.Length; i++)
                {
                    routes[i].AddFirst(new Move(current.allSteps[i].move));
                }
                current = current.prev;
            }

            var ans = new SinglePlan[problem.Length];
            for (int i = 0; i < ans.Length; i++)
                ans[i] = new SinglePlan(routes[i], i);
            return ans;
        }

        private bool CheckIfLegal(MDDNode to1, MDDNode to2)
        {
            return to1.move.IsColliding(to2.move) == false;
        }

        private bool IsLegalMove(A_Star_MDDs_Node to)
        {
            if (to == null)
                return false;
            if (to.prev == null)
                return true;
            for (int i = 0; i < problem.Length; i++)
            {
                for (int j = i + 1; j < to.allSteps.Length; j++)
                {
                    if (CheckIfLegal(to.allSteps[i], to.allSteps[j]) == false)
                        return false;
                }
            }
            return true;
        }
    }

    class A_Star_MDDs_Node : IComparable<IBinaryHeapItem>, IBinaryHeapItem
    {
        /// <summary>
        /// The last move of all agents that have already moved in this turn.
        /// Used for making sure the next agent move doesn't collide with moves already made.
        /// Used while generating this node, nullified when done.
        /// </summary>
        public HashSet<TimedMove> currentMoves;
        public MDDNode[] allSteps;
        public A_Star_MDDs_Node prev;
        public int conflictCount;
        public Dictionary<int, int> conflictCounts;
        public Dictionary<int, List<int>> conflictTimes;
        int binaryHeapIndex;

        public bool alreadyExpanded;
        /// <summary>
        /// Starts at zero, incremented after a node is expanded once. Set on Expand.
        /// </summary>
        public ushort targetDeltaConflictCount = 0;
        /// <summary>
        /// Remaining delta conflict count towards targetDeltaConflictCount. Reset on Expand.
        /// </summary>
        public ushort remainingDeltaConflictCount;
        /// <summary>
        /// For each MDD node and each child it has, the effect of that choosing that child on the conflict count.
        /// byte.MaxValue means this is an illegal move. Only computed on demand.
        /// </summary>
        protected byte[][] singleAgentDeltaConflictCounts;
        /// <summary>
        /// Only computed on demand
        /// </summary>
        protected ushort maxDeltaConflictCount;
        /// <summary>
        /// Per each MDD node and delta conflict count, has 1 if that delta F is achievable by choosing child MDD nodes starting from this one on,
        /// -1 if it isn't, and 0 if we don't know yet.
        /// Only computed on demand
        /// </summary>
        protected sbyte[][] conflictCountLookup;

        /// <summary>
        /// From generated nodes. Allows expansion table to be garbage collected before all generated nodes are expanded.
        /// </summary>
        public void ClearExpansionData()
        {
            this.singleAgentDeltaConflictCounts = null;
            this.conflictCountLookup = null;
            this.currentMoves = null;
        }

        /// <summary>
        /// Counts the number of times this node collides with each agent move in the conflict avoidance table.
        /// </summary>
        /// <param name="CAT"></param>
        /// <returns></returns>
        public virtual void IncrementConflictCounts(ConflictAvoidanceTable CAT)
        {
            foreach (var mddNode in this.allSteps)
            {
                mddNode.move.IncrementConflictCounts(CAT, this.conflictCounts, this.conflictTimes);
            }
        }

        /// <summary>
        /// Returns whether all possible f values were generated from this node already
        /// </summary>
        /// <returns></returns>
        public bool hasMoreChildren()
        {
            return this.targetDeltaConflictCount <= this.maxDeltaConflictCount;
        }

        public bool IsAlreadyExpanded()
        {
            return alreadyExpanded;
        }

        public bool hasChildrenForCurrentDeltaConflictCount(int agentNum = 0)
        {
            return existsChildForConflictCount(agentNum, this.remainingDeltaConflictCount);
        }

        /// <summary>
        /// An MDD child node was chosen between calculating the singleAgentDeltaConflictCounts and this call.
        /// Using the data that describes its delta conflict count potential before the move.
        /// </summary>
        /// <param name="mddIndex">which mdd's node was expanded to create this A*_MDDs node</param>
        /// <param name="childIndex">which of the mdd node's children was just chosen</param>
        public void UpdateRemainingDeltaConflictCount(int mddIndex, int childIndex)
        {
            if (this.remainingDeltaConflictCount == ushort.MaxValue)
                Trace.Assert(false,
                             $"Remaining deltaConflictCount is ushort.MaxValue, a reserved value with special meaning. agentIndex={mddIndex}");

            byte lastMoveDeltaConflictCount = this.singleAgentDeltaConflictCounts[mddIndex][childIndex];
            if (lastMoveDeltaConflictCount != byte.MaxValue && this.remainingDeltaConflictCount >= lastMoveDeltaConflictCount)
                this.remainingDeltaConflictCount -= lastMoveDeltaConflictCount;
            else
                this.remainingDeltaConflictCount = ushort.MaxValue; // Either because last move was illegal or because the delta F from the last move was more than the entire remaining delta F budget
        }

        /// <summary>
        /// Recursive func. Kind of dynamic programming as it updates the lookup table as it goes to refrain from computing answers twice.
        /// </summary>
        /// <param name="mddIndex"></param>
        /// <param name="remainingTargetDeltaF"></param>
        /// <returns></returns>
        protected bool existsChildForConflictCount(int mddIndex, ushort remainingTargetDeltaConflictCount)
        {
            // Stopping conditions:
            if (mddIndex == this.allSteps.Length)
            {
                if (remainingTargetDeltaConflictCount == 0)
                    return true;
                return false;
            }

            if (conflictCountLookup[mddIndex][remainingTargetDeltaConflictCount] != 0) // Answer known (arrays are initialized to zero). TODO: Replace the magic.
            {
                return conflictCountLookup[mddIndex][remainingTargetDeltaConflictCount] == 1; // Return known answer. TODO: Replace the magic
            }

            // Recursive actions:
            for (int i = 0; i < this.allSteps[mddIndex].children.Count; i++)
            {
                if (singleAgentDeltaConflictCounts[mddIndex][i] > remainingTargetDeltaConflictCount) // Small optimization - no need to make the recursive
                                                                                                     // call just to request a negative target from it and
                                                                                                     // get false (because we assume the heuristic function
                                                                                                     // is consistent)
                    continue;
                if (existsChildForConflictCount(mddIndex + 1,
                                                (byte)(remainingTargetDeltaConflictCount - singleAgentDeltaConflictCounts[mddIndex][i])))
                {
                    conflictCountLookup[mddIndex][remainingTargetDeltaConflictCount] = 1;
                    return true;
                }
            }
            conflictCountLookup[mddIndex][remainingTargetDeltaConflictCount] = -1;
            return false;
        }

        /// <summary>
        /// Calculates for each MDD node and each of its children, the effect of that move on the conflict count.
        /// Also calcs maxDeltaConflictCount.
        /// Note: Currently avoids the CAT's avoidanceGoal. To compute each individual agent's effect on the count of groups we conflict with would require
        /// tracking conflicts as a set of groups we conflict with instead of as a sum of conflicts, and would require 2^(num agents) cells in each singleAgentDeltaConflictCounts[i].
        /// </summary>
        /// <param name="CAT"></param>
        /// <returns></returns>
        public void calcSingleAgentDeltaConflictCounts(ConflictAvoidanceTable CAT)
        {
            // Init
            this.singleAgentDeltaConflictCounts = new byte[this.allSteps.Length][];
            for (int i = 0; i < this.allSteps.Length; i++)
            {
                this.singleAgentDeltaConflictCounts[i] = new byte[this.allSteps[i].children.Count];
            }

            int conflictCountAfter;

            this.maxDeltaConflictCount = 0;

            // Set values
            for (int i = 0; i < this.allSteps.Length; i++)
            {
                int singleAgentMaxLegalDeltaConflictCount = -1;

                foreach ((int childIndex, MDDNode child) in this.allSteps[i].children.Enumerate())
                {
                    if (CAT != null)
                    {
                        conflictCountAfter = CAT[child.move].Count;
                    }
                    else
                        conflictCountAfter = 0;

                    singleAgentDeltaConflictCounts[i][childIndex] = (byte)conflictCountAfter;
                    singleAgentMaxLegalDeltaConflictCount = Math.Max(singleAgentMaxLegalDeltaConflictCount, singleAgentDeltaConflictCounts[i][childIndex]);
                }

                if (singleAgentMaxLegalDeltaConflictCount == -1) // No legal action for this agent, so no legal children exist for this node
                {
                    this.maxDeltaConflictCount = 0; // Can't make it negative without widening the field.
                    break;
                }

                this.maxDeltaConflictCount += (byte)singleAgentMaxLegalDeltaConflictCount;
            }

            conflictCountLookup = new sbyte[this.allSteps.Length][];
            for (int i = 0; i < conflictCountLookup.Length; i++)
            {
                conflictCountLookup[i] = new sbyte[this.maxDeltaConflictCount + 1];  // Towards the last agents most of the row will be wasted (the last one can do delta F of 0 or 1),
                                                                                     // but it's easier than fiddling with array sizes
            }
        }

        public A_Star_MDDs_Node(MDDNode[] allSteps, A_Star_MDDs_Node prevStep)
        {
            this.allSteps = allSteps;
            this.prev = prevStep;
            this.currentMoves = null;  // All non-intermediate nodes have currentMoves == null
            this.conflictCount = 0;

            // Initialize conflict tracking data structures
            this.conflictCounts = new Dictionary<int, int>();
            this.conflictTimes = new Dictionary<int, List<int>>();
        }

        /// <summary>
        /// Copy constructor
        /// </summary>
        public A_Star_MDDs_Node(A_Star_MDDs_Node cpy, bool createIntermediate)
        {
            this.allSteps = new MDDNode[cpy.allSteps.Length];
            for (int i = 0; i < allSteps.Length; i++)
            {
                this.allSteps[i] = cpy.allSteps[i];
            }
            this.prev = cpy.prev;
            if (cpy.currentMoves != null)
            {
                // cpy is an intermediate node
                if (createIntermediate)
                    this.currentMoves = new HashSet<TimedMove>(cpy.currentMoves);
                else
                    this.currentMoves = cpy.currentMoves;  // We're not going to add anything currentMoves
            }
            else
                // cpy is a concrete node
                this.currentMoves = new HashSet<TimedMove>(capacity: cpy.allSteps.Length);

            // The conflictTimes and conflictCounts are only copied later if necessary.

            alreadyExpanded = false; // Creating a new unexpanded node from cpy

            // For intermediate nodes created during expansion (fully expanded nodes have these fields recalculated when they're expanded)
            targetDeltaConflictCount = cpy.targetDeltaConflictCount;  // Just to ease debugging
            remainingDeltaConflictCount = cpy.remainingDeltaConflictCount;
            singleAgentDeltaConflictCounts = cpy.singleAgentDeltaConflictCounts; // For the UpdateRemainingDeltaConflictCount call on temporary nodes.
                                                                                 // Notice that after an agent is moved its row won't be up-to-date.
            conflictCountLookup = cpy.conflictCountLookup; // For the hasChildrenForCurrentDeltaConflictCount call on temporary nodes.
                                                           // Notice that after an agent is moved, all rows up to and including the one of the agent that moved
                                                           // won't be up-to-date.
            maxDeltaConflictCount = cpy.maxDeltaConflictCount; // Not necessarily achievable after some of the agents moved.
                                                               // The above is OK because we won't be using data for agents that already moved.
        }

        /// <summary>
        /// Only compares the steps.
        /// </summary>
        /// <param name="obj"></param>
        /// <returns></returns>
        public override bool Equals(object obj)
        {
            if (obj == null)
                return false;
            A_Star_MDDs_Node comp = (A_Star_MDDs_Node)obj;
            return this.allSteps.SequenceEqual<MDDNode>(comp.allSteps);
        }

        /// <summary>
        /// Only uses the steps
        /// </summary>
        /// <returns></returns>
        public override int GetHashCode()
        {
            unchecked
            {
                int code = 0;
                for (int i = 0; i < allSteps.Length; i++)
                {
                    code += allSteps[i].GetHashCode() * Constants.PRIMES_FOR_HASHING[i % Constants.PRIMES_FOR_HASHING.Length];
                }
                return code;
            }
        }

        public int GetDepth() { return allSteps[0].move.time; }

        /// <summary>
        /// Updates the conflictCount member according to given CATs. Table may be null.
        /// </summary>
        /// <param name="CAT"></param>
        public void UpdateConflicts(ConflictAvoidanceTable CAT)
        {
            if (this.prev == null)
                return;
            if (CAT != null)
            {
                for (int i = 0; i < allSteps.Length; i++)
                {
                    if (CAT.ContainsKey(allSteps[i].move))
                        conflictCount += CAT[allSteps[i].move].Count;
                }
            }
        }

        /// <summary>
        /// BH_Item implementation
        /// </summary>
        /// <returns></returns>
        public int GetIndexInHeap() { return binaryHeapIndex; }

        /// <summary>
        /// BH_Item implementation
        /// </summary>
        /// <returns></returns>
        public void SetIndexInHeap(int index) { binaryHeapIndex = index; }

        /// <summary>
        /// Prefers fewer conflicts. If the number of conflicts is the same, prefers more depth.
        /// </summary>
        /// <param name="other"></param>
        /// <returns></returns>
        public int CompareTo(IBinaryHeapItem other)
        {
            A_Star_MDDs_Node that = (A_Star_MDDs_Node)other;
            if (this.conflictCount + this.targetDeltaConflictCount < that.conflictCount + that.targetDeltaConflictCount)
                return -1;
            if (this.conflictCount + this.targetDeltaConflictCount > that.conflictCount + that.targetDeltaConflictCount)
                return 1;

            if (this.GetDepth() > that.GetDepth())
                return -1;
            if (this.GetDepth() < that.GetDepth())
                return 1;

            return 0;
        }
    }

    class A_Star_MDDs_Expander
    {
        private A_Star_MDDs_Node a_star_mdd_node;
        /// <summary>
        /// For each MDD, the index of the next child in <children> to choose
        /// </summary>
        int[] chosenChild;

        public A_Star_MDDs_Expander() { }

        public A_Star_MDDs_Expander(A_Star_MDDs_Node a_star_mdd_node)
        {
            this.a_star_mdd_node = a_star_mdd_node;
            this.chosenChild = new int[a_star_mdd_node.allSteps.Length];
            foreach (MDDNode mddNode in a_star_mdd_node.allSteps)
            {
                if (mddNode.children.Count == 0)
                {
                    chosenChild[0] = -1;
                    break;
                }
            }
        }

        public void Setup(A_Star_MDDs_Node a_star_mdd_node)
        {
            this.a_star_mdd_node = a_star_mdd_node;
            this.chosenChild = new int[a_star_mdd_node.allSteps.Length];
            foreach (MDDNode mddNode in a_star_mdd_node.allSteps)
            {
                if (mddNode.children.Count == 0)
                {
                    this.chosenChild[0] = -1;
                    break;
                }
            }
        }

        /// <summary>
        /// 
        /// </summary>
        /// <returns>The next child, or null if there aren't any more</returns>
        public A_Star_MDDs_Node GetNextChild()
        {
            if (this.chosenChild[0] == -1)
                return null;
            var mddNodes = new MDDNode[a_star_mdd_node.allSteps.Length];
            for (int i = 0; i < mddNodes.Length; i++)
            {
                mddNodes[i] = a_star_mdd_node.allSteps[i].children.ElementAt(this.chosenChild[i]);
            }
            SetNextChildIndices();
            return new A_Star_MDDs_Node(mddNodes, a_star_mdd_node);
        }

        /// <summary>
        /// Increments the chosenChild indices array by 1
        /// </summary>
        private void SetNextChildIndices()
        {
            SetNextChildIndices(this.chosenChild.Length - 1);
        }

        private void SetNextChildIndices(int agentNum)
        {
            if (agentNum == -1)
                this.chosenChild[0] = -1;
            else if (this.chosenChild[agentNum] < a_star_mdd_node.allSteps[agentNum].children.Count - 1)
                this.chosenChild[agentNum]++;
            else
            {
                this.chosenChild[agentNum] = 0;
                SetNextChildIndices(agentNum - 1);
            }
        }
    }
}