WorldStateForPartialExpansion.cs

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

namespace mapf
{
    class WorldStateForPartialExpansion : WorldState
    {
        public bool alreadyExpanded;
        /// <summary>
        /// Starts at zero, incremented after a node is expanded once. Set on Expand.
        /// </summary>
        public ushort targetDeltaF;
        /// <summary>
        /// Remaining delta F towards targetDeltaF. Reset on Expand.
        /// </summary>
        public ushort remainingDeltaF;
        /// <summary>
        /// For each agent and each direction it can go, the effect of that move on F
        /// byte.MaxValue means this is an illegal move. Only computed on demand.
        /// </summary>
        protected byte[][] singleAgentDeltaFs;
        /// <summary>
        /// Only computed on demand
        /// </summary>
        protected ushort maxDeltaF;
        public enum DeltaFAchievable : sbyte
        {
            YES = 1,
            NO = -1,
            NOT_YET_COMPUTED = 0
        }
        /// <summary>
        /// Per each agent and delta F, has 1 if that delta F is achievable by moving the agents starting from this one on,
        /// -1 if it isn't, and 0 if we don't know yet.
        /// Only computed on demand
        /// </summary>
        protected DeltaFAchievable[][] fLookup;
        /// <summary>
        /// The node's SIC heuristic estimate
        /// </summary>
        public int sic;

        /// <summary>
        /// Create a state with the given state for every agent.
        /// </summary>
        /// <param name="allAgentsState"></param>
        /// <param name="minDepth"></param>
        /// <param name="minCost"></param>
        public WorldStateForPartialExpansion(AgentState[] allAgentState, int minDepth = -1,
                                             int minCost = -1, MDDNode mddNode = null) :
            base(allAgentState, minDepth, minCost, mddNode)
        {
            this.alreadyExpanded = false;
            this.maxDeltaF = 0;
            this.singleAgentDeltaFs = null;
            this.fLookup = null;
        }

        /// <summary>
        /// Copy constructor
        /// </summary>
        /// <param name="cpy"></param>
        public WorldStateForPartialExpansion(WorldStateForPartialExpansion cpy)
            : base(cpy)
        {
            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)
            remainingDeltaF = cpy.remainingDeltaF;
            singleAgentDeltaFs = cpy.singleAgentDeltaFs; // For the UpdateRemainingDeltaF call on temporary nodes.
                                                         // Notice that after an agent is moved its row won't be up-to-date.
            fLookup = cpy.fLookup; // For the hasChildrenForCurrentDeltaF 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.
            maxDeltaF = cpy.maxDeltaF; // 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>
        /// From generated nodes. Allows expansion table to be garbage collected before all generated nodes are expanded.
        /// </summary>
        public void ClearExpansionData()
        {
            this.singleAgentDeltaFs = null;
            this.fLookup = null;
        }

        public override string ToString()
        {
            return $"{base.ToString()} (sic:{this.sic} target deltaF: {this.targetDeltaF})";
        }

        public delegate bool ValidityChecker(TimedMove move, IReadOnlyDictionary<TimedMove, int> currentMoves, int makespan, int agentIndex, WorldState node, WorldState intermediateNode);

        private static readonly IReadOnlyDictionary<TimedMove, int> noMoves = new Dictionary<TimedMove, int>();

        /// <summary>
        /// Calculates for each agent and each direction it can go, the effect of that move on F. Illegal moves get byte.MaxValue.
        /// Also calcs maxDeltaF.
        /// Implicitly uses the SIC heuristic.
        /// </summary>
        /// <param name="problem">For GetSingleAgentOptimalCost</param>
        /// <param name="isValid"></param>
        /// <returns></returns>
        public void calcSingleAgentDeltaFs(ProblemInstance problem, ValidityChecker isValid)
        {
            // Init
            this.singleAgentDeltaFs = new byte[allAgentsState.Length][];
            for (int i = 0; i < singleAgentDeltaFs.Length; i++)
            {
                this.singleAgentDeltaFs[i] = new byte[Constants.NUM_ALLOWED_DIRECTIONS];
            }

            int hBefore, hAfter;

            this.maxDeltaF = 0;

            // Set values
            for (int i = 0; i < allAgentsState.Length; i++)
            {
                hBefore = problem.GetSingleAgentOptimalCost(allAgentsState[i]);

                int singleAgentMaxLegalDeltaF = -1;

                foreach (TimedMove check in allAgentsState[i].lastMove.GetNextMoves())
                {
                    if (isValid(check, noMoves, this.makespan + 1, i, this, this) == false)  // Is this move by itself invalid because of constraints or obstacles
                    {
                        singleAgentDeltaFs[i][(int)check.direction] = byte.MaxValue;
                    }
                    else
                    {
                        hAfter = problem.GetSingleAgentOptimalCost(allAgentsState[i].agent.agentNum, check);

                        if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG)
                        {
                            if (hBefore != 0)
                                singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain
                            else if (hAfter != 0) // If agent moved from its goal we must count and add all the steps it was stationed at the goal, since they're now part of its g difference
                                singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + makespan - allAgentsState[i].arrivalTime + 1);
                            else
                                singleAgentDeltaFs[i][(int)check.direction] = 0; // This is a WAIT move at the goal.
                        }
                        else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE)
                        {
                            if (hBefore == 0 && hAfter == 0)
                                singleAgentDeltaFs[i][(int)check.direction] = 0; // This is a WAIT move at the goal.
                            else
                                singleAgentDeltaFs[i][(int)check.direction] = (byte)(hAfter - hBefore + 1); // h difference + g difference in this specific domain
                        }
                        singleAgentMaxLegalDeltaF = Math.Max(singleAgentMaxLegalDeltaF, singleAgentDeltaFs[i][(int)check.direction]);
                    }
                }

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

                this.maxDeltaF += (byte)singleAgentMaxLegalDeltaF;
            }

            fLookup = new DeltaFAchievable[allAgentsState.Length][];
            for (int i = 0; i < fLookup.Length; i++)
            {
                fLookup[i] = new DeltaFAchievable[this.maxDeltaF + 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
            }
        }

        /// <summary>
        /// Returns whether all possible f values were generated from this node already.
        /// Assumes calcSingleAgentDeltaFs was called earlier.
        /// </summary>
        /// <returns></returns>
        public bool hasMoreChildren()
        {
            return this.targetDeltaF <= this.maxDeltaF;
        }

        public bool IsAlreadyExpanded()
        {
            return alreadyExpanded;
        }

        /// <summary>
        /// Assumes calcSingleAgentDeltaFs was called earlier.
        /// </summary>
        /// <param name="agentNum"></param>
        /// <returns></returns>
        public bool hasChildrenForCurrentDeltaF(int agentNum = 0)
        {
            return existsChildForF(agentNum, this.remainingDeltaF);
        }

        /// <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="agentNum"></param>
        /// <param name="remainingTargetDeltaF"></param>
        /// <returns></returns>
        protected bool existsChildForF(int agentNum, ushort remainingTargetDeltaF)
        {
            // Stopping conditions:
            if (agentNum == allAgentsState.Length)
            {
                if (remainingTargetDeltaF == 0)
                    return true;
                return false;
            }

            if (fLookup[agentNum][remainingTargetDeltaF] != DeltaFAchievable.NOT_YET_COMPUTED) // Answer known (arrays are initialized to zero).
            {
                return fLookup[agentNum][remainingTargetDeltaF] == DeltaFAchievable.YES; // Return known answer.
            }

            // Recursive actions:
            for (int direction = 0; direction < Constants.NUM_ALLOWED_DIRECTIONS; direction++)
            {
                if (singleAgentDeltaFs[agentNum][direction] > remainingTargetDeltaF) // 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 (existsChildForF(agentNum + 1, (byte)(remainingTargetDeltaF - singleAgentDeltaFs[agentNum][direction])))
                {
                    fLookup[agentNum][remainingTargetDeltaF] = DeltaFAchievable.YES;
                    return true;
                }
            }
            fLookup[agentNum][remainingTargetDeltaF] = DeltaFAchievable.NO;
            return false;
        }

        /// <summary>
        /// An agent was moved between calculating the singleAgentDeltaFs and this call.
        /// Using the data that describes its delta F potential before the move.
        /// </summary>
        /// <param name="agentIndex"></param>
        public void UpdateRemainingDeltaF(int agentIndex)
        {
            if (this.remainingDeltaF == ushort.MaxValue)
                Trace.Assert(false,
                             $"Remaining deltaF is ushort.MaxValue, a reserved value with special meaning. agentIndex={agentIndex}");

            byte lastMoveDeltaF = this.singleAgentDeltaFs[agentIndex][(int)this.allAgentsState[agentIndex].lastMove.direction];
            if (lastMoveDeltaF != byte.MaxValue && this.remainingDeltaF >= lastMoveDeltaF)
                this.remainingDeltaF -= lastMoveDeltaF;
            else
                this.remainingDeltaF = 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>
        /// For fully expanded nodes.
        /// Notice ClearExpansionData does a similar thing, but for different reasons.
        /// </summary>
        public override void Clear()
        {
            this.alreadyExpanded = false; // Enables reopening
            // The following info could be reused when reopening the node, saving the time it takes to generate it,
            // but reopening a node is rare in our domain, and the memory saved can be significant
            this.fLookup = null; // Save a lot of memory
            this.singleAgentDeltaFs = null; // Save some more memory
            this.targetDeltaF = 0;
            this.remainingDeltaF = 0;
            //this.h -= this.hBonus; // Reset the h
            //this.hBonus = 0;
        }

        public override int f
        {
            get
            {
                return Math.Max(this.g + this.h,
                                this.g + this.sic + this.targetDeltaF);
            }
        }
    }
}