CBS.cs

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

namespace mapf;

/// <summary>
/// Merges agents if they conflict more times than the given threshold in the CT nodes
/// from the root to the current CT nodes only.
/// </summary>
public class CBS : ICbsSolver, IHeuristicSolver<CbsNode>, IIndependenceDetectionSolver
{
    /// <summary>
    /// For each agent, map sets of constraints to MDDs
    /// </summary>
    public Dictionary<CbsCacheEntry, MDD>[] mddCache;

    /// <summary>
    /// For each agent, map sets of constraints to a dictionary that
    /// maps each level (timestep) of its mdd to a narrowness degree.
    /// Non-narrow levels are omitted.
    /// </summary>
    public Dictionary<CbsCacheEntry, Dictionary<int, MDD.LevelNarrowness>>[] mddNarrownessValuesCache;

    protected ProblemInstance instance;
    public OpenList<CbsNode> openList;
    /// <summary>
    /// Might as well be a HashSet. We don't need to retrieve from it.
    /// TODO: Consider a closedList for single agent paths under an unordered set of constraints.
    ///       It would get more hits.
    /// </summary>
    public Dictionary<CbsNode, CbsNode> closedList;
    protected ILazyHeuristic<CbsNode> heuristic;
    protected int highLevelExpanded;
    protected int highLevelGenerated;
    protected int closedListHits;
    protected int partialExpansions;
    protected int bypasses;
    protected int nodesExpandedWithGoalCost;
    protected int bypassLookAheadNodesCreated;
    protected int cardinalLookAheadNodesCreated;
    protected int conflictsBypassed;
    protected int cardinalConflictSplits;
    protected int semiCardinalConflictSplits;
    protected int nonCardinalConflictSplits;
    public int mddsBuilt;
    public int mddsAdapted;
    protected int restarts;
    protected int pathMaxBoosts;
    protected int reversePathMaxBoosts;
    protected int pathMaxPlusBoosts;
    protected int surplusNodesAvoided;
    public int mddCacheHits;
    public double timePlanningPaths;
    public double timeBuildingMdds;
    // TODO: Count shuffles
    protected int accHLExpanded;
    protected int accHLGenerated;
    protected int accClosedListHits;
    protected int accPartialExpansions;
    protected int accBypasses;
    protected int accNodesExpandedWithGoalCost;
    protected int accBypassLookAheadNodesCreated;
    protected int accCardinalLookAheadNodesCreated;
    protected int accConflictsBypassed;
    protected int accCardinalConflictSplits;
    protected int accSemiCardinalConflictSplits;
    protected int accNonCardinalConflictSplits;
    protected int accMddsBuilt;
    protected int accMddsAdapted;
    protected int accRestarts;
    protected int accPathMaxBoosts;
    protected int accReversePathMaxBoosts;
    protected int accPathMaxPlusBoosts;
    protected int accSurplusNodesAvoided;
    protected int accMddCacheHits;
    protected double accTimePlanningPaths;
    protected double accTimeBuildingMdds;

    public int solutionCost;
    /// <summary>
    /// The difference between the solution's cost and the f of the root node.
    /// Notice root.g != 0 in CBS.
    /// </summary>
    protected int solutionDepth;
    public Run runner;
    protected CbsNode goalNode;
    protected Plan solution;
    protected Dictionary<int, int> externalConflictCounts;
    protected Dictionary<int, List<int>> externalConflictTimes;
    /// <summary>
    /// Nodes with a higher F aren't generated. As a result, goal nodes with a higher cost
    /// won't be found.
    /// </summary>
    protected int maxSolutionCost;
    /// <summary>
    /// Goal Nodes with with a lower cost aren't considered a goal. Used directly by CbsNode.
    /// </summary>
    public int minSolutionCost;
    /// <summary>
    /// Search is stopped when the minimum F in the open list reaches the target,
    /// regardless of whether a goal node was found. Note maxSolutionCost stops the search when
    /// the same F value is exhausted from the open list later.
    /// </summary>
    public int targetF {
        get
        {
            return this.m_targetF;
        }
        set
        {
            this.maxSolutionCost = Math.Min(this.maxSolutionCost, value);  // No need to generate nodes with a higher F
            this.m_targetF = value;
        }
    }
    protected int m_targetF;
    /// <summary>
    /// Search is stopped when the low level generated nodes count exceeds the cap
    /// </summary>
    public int lowLevelGeneratedCap { set; get; }
    /// <summary>
    /// Search is stopped when the millisecond count exceeds the cap
    /// </summary>
    public int milliCap { set; get; }
    protected ICbsSolver solver;
    protected ICbsSolver singleAgentSolver;
    public int mergeThreshold;
    public int minSolutionTimeStep;
    protected int maxSizeGroup;
    protected int accMaxSizeGroup;
    public BypassStrategy bypassStrategy;
    public bool doMalte;
    public ConflictChoice conflictChoice;
    public bool disableTieBreakingByMinOpsEstimate;
    public int lookaheadMaxExpansions;
    public enum ConflictChoice : byte
    {
        FIRST = 0,
        MOST_CONFLICTING_SMALLEST_AGENTS,
        CARDINAL_MDD,
        CARDINAL_LOOKAHEAD,
        CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP,
        CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP
    }
    public enum BypassStrategy : byte
    {
        NONE = 0,
        FIRST_FIT_LOOKAHEAD,
        BEST_FIT_LOOKAHEAD
    }
    protected bool mergeCausesRestart;
    private bool useOldCost;
    public bool replanSameCostWithMdd;
    public bool cacheMdds;
    public bool useCAT;

    public ConflictAvoidanceTable externalCAT;
    public ISet<CbsConstraint> externalConstraints;
    /// <summary>
    /// Note that in this implementation, positive constraints currenly don't act as negative constraints for all other agents, regretably.
    /// They only rule out every other location for the agent at that time step.
    /// </summary>
    public ISet<CbsConstraint> externalPositiveConstraints;

    /// <summary>
    /// 
    /// </summary>
    /// <param name="singleAgentSolver"></param>
    /// <param name="generalSolver"></param>
    /// <param name="mergeThreshold"></param>
    /// <param name="bypassStrategy"></param>
    /// <param name="doMalte"></param>
    /// <param name="conflictChoice"></param>
    /// <param name="heuristic">Assumed to be expensive to compute. Used as late as possible</param>
    /// <param name="disableTieBreakingByMinOpsEstimate"></param>
    /// <param name="lookaheadMaxExpansions"></param>
    /// <param name="mergeCausesRestart"></param>
    /// <param name="replanSameCostWithMdd"></param>
    /// <param name="cacheMdds"></param>
    /// <param name="useOldCost"></param>
    /// <param name="useCAT"></param>
    public CBS(ICbsSolver singleAgentSolver, ICbsSolver generalSolver,
                                int mergeThreshold = -1,
                                BypassStrategy bypassStrategy = BypassStrategy.NONE,
                                bool doMalte = false,
                                ConflictChoice conflictChoice = ConflictChoice.FIRST,
                                ILazyHeuristic<CbsNode> heuristic = null,
                                bool disableTieBreakingByMinOpsEstimate = true,
                                int lookaheadMaxExpansions = 1,
                                bool mergeCausesRestart = false,
                                bool replanSameCostWithMdd = false,
                                bool cacheMdds = false,
                                bool useOldCost = false,
                                bool useCAT = true
        )
    {
        this.closedList = new Dictionary<CbsNode, CbsNode>();
        if (heuristic == null)
            this.openList = new OpenList<CbsNode>(this);
        else
            this.openList = new DynamicLazyOpenList<CbsNode>(this, heuristic);
        this.mergeThreshold = mergeThreshold;
        this.solver = generalSolver;
        this.singleAgentSolver = singleAgentSolver;
        this.bypassStrategy = bypassStrategy;
        this.doMalte = doMalte;
        this.conflictChoice = conflictChoice;
        this.heuristic = heuristic;
        if (Constants.costFunction != Constants.CostFunction.SUM_OF_COSTS)
        {
            Trace.Assert(conflictChoice != ConflictChoice.CARDINAL_MDD &&
                         conflictChoice != ConflictChoice.CARDINAL_LOOKAHEAD &&
                         conflictChoice != ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP &&
                         conflictChoice != ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP,  // TODO: Might be OK. Need to look at it.
		                 "Under makespan, increasing the cost for a single agent might not increase the cost for the solution." +
		                 "Before this strategy is enabled we need to add a consideration of whether the agent whose cost will " +
		                 "increase has the highest cost in the solution first");
        }
        this.disableTieBreakingByMinOpsEstimate = disableTieBreakingByMinOpsEstimate;
        this.lookaheadMaxExpansions = lookaheadMaxExpansions;
        this.mergeCausesRestart = mergeCausesRestart;
        this.replanSameCostWithMdd = replanSameCostWithMdd;
        this.cacheMdds = cacheMdds;
        this.useOldCost = useOldCost;
        this.useCAT = useCAT;
    }

    /// <summary>
    /// Implements IIndependenceDetection.Setup for new groups
    /// </summary>
    /// <param name="problemInstance"></param>
    /// <param name="runner"></param>
    /// <param name="CAT"></param>
    /// <param name="parentGroup1Cost"></param>
    /// <param name="parentGroup2Cost"></param>
    /// <param name="parentGroup1Size"></param>
    public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT,
                        int parentGroup1Cost, int parentGroup2Cost, int parentGroup1Size)
    {
        this.Setup(problemInstance, -1, runner, CAT, null, null, parentGroup1Cost + parentGroup2Cost);  // TODO: Support a makespan cost function
        // I think we don't want to merge the agents in each group. We'd be left with two large meta-agents,
        // and in CBS one conflict isn't a reason to merge agents. The groups are arbitrarily large so we can't
        // even increment their conflict counts toward the merge threshold.
    }

    /// <summary>
    /// Implements IIndependenceDetection.Setup for replanning groups to resolve a conflict
    /// </summary>
    /// <param name="problemInstance"></param>
    /// <param name="runner"></param>
    /// <param name="CAT"></param>
    /// <param name="targetCost">/// </param>
    /// <param name="illegalMoves"></param>
    public void Setup(ProblemInstance problemInstance, Run runner, ConflictAvoidanceTable CAT,
                        int targetCost, ISet<TimedMove> illegalMoves)
    {
        // Turn the set of reserved/illegal moves into constraints for every agent. Not the prettiest solution.
        // TODO: Instead, maybe add the agents in the reservation table into the problem instance, and add positive
        //       constraints for them along their entire path. Would require changing IIndependenceDetectionSolver.Setup
        //       to specify the agents the reserved moves belong to.
        var constraints = new HashSet<CbsConstraint>(illegalMoves.Count * problemInstance.agents.Length);
        foreach (var illegalMove in illegalMoves)
        {
            foreach (var agentState in problemInstance.agents)
            {
                constraints.Add(new CbsConstraint(agentState.agent.agentNum, illegalMove));
            }
        }
        this.Setup(problemInstance, illegalMoves.Max(move => move.time), runner, CAT, constraints, null, targetCost, targetCost);
    }

    /// <summary>
    /// Implements ICbsSolver.Setup.
    /// </summary>
    /// <param name="problemInstance"></param>
    /// <param name="minSolutionTimeStep"></param>
    /// <param name="runner"></param>
    /// <param name="minSolutionCost"></param>
    /// <param name="maxSolutionCost"></param>
    /// <param name="mdd">Currently ignored. FIXME: Need to convert to array of MDDs to use.</param>
    public virtual void Setup(ProblemInstance problemInstance, int minSolutionTimeStep, Run runner,
        ConflictAvoidanceTable externalCAT, ISet<CbsConstraint> externalConstraints,
        ISet<CbsConstraint> externalPositiveConstraints,
        int minSolutionCost = -1, int maxSolutionCost = int.MaxValue, MDD mdd = null)
    {
        this.instance = problemInstance;
        this.runner = runner;
        if (this.openList is DynamicLazyOpenList<CbsNode>)
            ((DynamicLazyOpenList<CbsNode>)this.openList).runner = runner;

        this.ClearPrivateStatistics();
        this.solutionCost = 0;
        this.solutionDepth = -1;
        this.lowLevelGeneratedCap = int.MaxValue;
        this.targetF = int.MaxValue;
        this.milliCap = int.MaxValue;
        this.goalNode = null;
        this.solution = null;

        // CBS parameters
        this.minSolutionTimeStep = minSolutionTimeStep;
        this.minSolutionCost = minSolutionCost;
        this.maxSolutionCost = Math.Max(this.maxSolutionCost, maxSolutionCost);
        if (this.replanSameCostWithMdd)
            Trace.Assert(this.mergeThreshold == -1, "Using MDDs to replan same-cost paths is currently only supported for single agents");

        if (this.cacheMdds)
        {
            this.mddCache = new Dictionary<CbsCacheEntry, MDD>[instance.agents.Length];
            this.mddNarrownessValuesCache = new Dictionary<CbsCacheEntry, Dictionary<int, MDD.LevelNarrowness>>[instance.agents.Length];
            for (int i = 0; i < instance.agents.Length; i++)
            {
                this.mddCache[i] = new Dictionary<CbsCacheEntry, MDD>();
                this.mddNarrownessValuesCache[i] = new Dictionary<CbsCacheEntry, Dictionary<int, MDD.LevelNarrowness>>();
            }
        }

        this.externalCAT = externalCAT;
        CAT_U CAT = null;
        if (this.useCAT)
        {
            CAT = new CAT_U();
            if (externalCAT != null)
                CAT.Join(externalCAT);
        }

        this.externalConstraints = externalConstraints;
        this.externalPositiveConstraints = externalPositiveConstraints;
        if (externalPositiveConstraints == null && this.doMalte)
            externalPositiveConstraints = new HashSet<CbsConstraint>();

        this.SetGlobals();

        CbsNode root = new CbsNode(instance.agents.Length, this.solver, this.singleAgentSolver, this);  // Problem instance and various strategy data is all passed under 'this'.
        // Solve the root node
        bool solved = root.Solve(minSolutionTimeStep);

        if (solved)
        {
            if (root.f <= this.maxSolutionCost)
            {
                this.addToGlobalConflictCount(root);  // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict
                this.openList.Add(root);
                this.highLevelGenerated++;
                this.closedList.Add(root, root);
            }
            else
            {
                this.surplusNodesAvoided++;
            }
        }
    }

    /// <summary>
    /// Implements ISolver.Setup 
    /// </summary>
    /// <param name="problemInstance"></param>
    /// <param name="runner"></param>
    public virtual void Setup(ProblemInstance problemInstance, Run runner)
    {
        this.Setup(problemInstance, 0, runner, null, null, null);
    }

    public IHeuristicCalculator<CbsNode> GetHeuristic()
    {
        return this.heuristic;
    }

    public ICbsSolver GetSolver()
    {
        return this.solver;
    }

    public Dictionary<int, int> GetExternalConflictCounts()
    {
        throw new NotImplementedException(); // For now. Also need to take care of generalised goal nodes!
    }

    public Dictionary<int, List<int>> GetConflictTimes()
    {
        throw new NotImplementedException(); // For now. Also need to take care of generalised goal nodes!
    }

    public ProblemInstance GetProblemInstance()
    {
        return this.instance;
    }

    public void Clear()
    {
        this.openList.Clear();
        this.closedList.Clear();
        this.solver.Clear();
        // Statistics are reset on Setup.
    }

    public virtual string GetName() 
    {
        string lowLevelSolvers;
        if (mergeThreshold == -1 || Object.ReferenceEquals(this.singleAgentSolver, this.solver))
            lowLevelSolvers = $"({this.singleAgentSolver})";
        else
            lowLevelSolvers = $"(single:{singleAgentSolver} multi:{solver})";
        string variants = "";
        if (this.bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD)
        {
            if (this.lookaheadMaxExpansions != int.MaxValue)
            {
                if (this.lookaheadMaxExpansions != 1)
                    variants += $" with first fit adoption max expansions: {this.lookaheadMaxExpansions}";
                else
                    variants += " + BP";
            }
            else
                variants += " with first fit adoption max expansions: $\\infty$"; // LaTeX infinity symbol
        }
        if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD)
        {
            if (this.lookaheadMaxExpansions == int.MaxValue)
                variants += " with infinite lookahead best fit adoption";
            else
                variants += $" with {this.lookaheadMaxExpansions} lookahead best fit adoption";
        }
        if (this.doMalte)
            variants += " with Malte";

        if (this.conflictChoice == ConflictChoice.FIRST)
            variants += " choosing the first conflict in CBS nodes";
        else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD)
            variants += " + PC";
        else if (this.conflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD)
            variants += " + PC choosing cardinal conflicts using lookahead";
        else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP)
            variants += " + PC MERGE EARLY MOST CONFLICTING SMALLEST GROUP";
        else if (this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP)
            variants += " + PC MERGE EARLY MOST CONFLICTING & SMALLEST GROUP";

        if (this.disableTieBreakingByMinOpsEstimate == true)
            variants += " without smart tie breaking";

        if (this.mergeCausesRestart == true && mergeThreshold != -1)
            variants += " with merge&restart";

        if (this.replanSameCostWithMdd)
            variants += " with replanning same cost paths with MDDs";

        if (this.cacheMdds)
            variants += " with caching MDDs";

        if (this.useOldCost)
            variants += " with using old path costs";

        if (this.useCAT == false)
            variants += " without a CAT";

        if (this.openList.GetType() != typeof(OpenList<CbsNode>))
        {
            variants += $" using {this.openList.GetName()}";
        }

        if (mergeThreshold == -1)
            return $"CBS/{lowLevelSolvers}{variants}";
        return $"MA-CBS-Local-{mergeThreshold}/{lowLevelSolvers}{variants}";
    }

    public override string ToString()
    {
        return GetName();
    }

    public int GetSolutionCost() { return this.solutionCost; }

    protected void ClearPrivateStatistics()
    {
        this.highLevelExpanded = 0;
        this.highLevelGenerated = 0;
        this.closedListHits = 0;
        this.partialExpansions = 0;
        this.bypasses = 0;
        this.nodesExpandedWithGoalCost = 0;
        this.bypassLookAheadNodesCreated = 0;
        this.cardinalLookAheadNodesCreated = 0;
        this.conflictsBypassed = 0;
        this.cardinalConflictSplits = 0;
        this.semiCardinalConflictSplits = 0;
        this.nonCardinalConflictSplits = 0;
        this.mddsBuilt = 0;
        this.mddsAdapted = 0;
        this.restarts = 0;
        this.pathMaxBoosts = 0;
        this.reversePathMaxBoosts = 0;
        this.pathMaxPlusBoosts = 0;
        this.maxSizeGroup = 1;
        this.surplusNodesAvoided = 0;
        this.mddCacheHits = 0;
        this.timePlanningPaths = 0;
        this.timeBuildingMdds = 0;
    }

    public virtual void OutputStatisticsHeader(TextWriter output)
    {
        output.Write(this.ToString() + " Expanded (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Generated (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Closed List Hits (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Partial Expansions (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Adoptions (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Nodes Expanded With Goal Cost (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Bypass Look Ahead Nodes Created (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Cardinal Look Ahead Nodes Created (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Conflicts Bypassed With Adoption (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Cardinal Conflict Splits (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Semi-Cardinal Conflict Splits (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Non-Cardinal Conflict Splits (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " MDDs Built (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " MDDs Adapted (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Restarts (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Path-Max Boosts (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Reverse Path-Max Boosts (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Path-Max Plus Boosts (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Max Group Size (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Surplus Nodes Avoided (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " MDD Cache Hits (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Time Planning Paths (HL)");
        output.Write(Run.RESULTS_DELIMITER);
        output.Write(this.ToString() + " Time Building MDDs (HL)");
        output.Write(Run.RESULTS_DELIMITER);

        this.solver.OutputStatisticsHeader(output);
        if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
            this.singleAgentSolver.OutputStatisticsHeader(output);

        this.openList.OutputStatisticsHeader(output);
    }

    public virtual void OutputStatistics(TextWriter output)
    {
        Console.WriteLine("Total Expanded Nodes (High-Level): {0}", this.GetHighLevelExpanded());
        Console.WriteLine("Total Generated Nodes (High-Level): {0}", this.GetHighLevelGenerated());
        Console.WriteLine("Closed List Hits (High-Level): {0}", this.closedListHits);
        Console.WriteLine("Partial Expansions (High-Level): {0}", this.partialExpansions);
        Console.WriteLine("Adoptions (High-Level): {0}", this.bypasses);
        Console.WriteLine("Nodes expanded with goal cost (High-Level): {0}", this.nodesExpandedWithGoalCost);
        Console.WriteLine("Bypass lookahead nodes created (High-Level): {0}", this.bypassLookAheadNodesCreated);
        Console.WriteLine("Cardinal lookahead nodes created (High-Level): {0}", this.cardinalLookAheadNodesCreated);
        Console.WriteLine("Conflicts Bypassed With Adoption (High-Level): {0}", this.conflictsBypassed);
        Console.WriteLine("Cardinal Conflicts Splits (High-Level): {0}", this.cardinalConflictSplits);
        Console.WriteLine("Semi-Cardinal Conflicts Splits (High-Level): {0}", this.semiCardinalConflictSplits);
        Console.WriteLine("Non-Cardinal Conflicts Splits (High-Level): {0}", this.nonCardinalConflictSplits);
        Console.WriteLine("MDDs Built (High-Level): {0}", this.mddsBuilt);
        Console.WriteLine("MDDs adapted (High-Level): {0}", this.mddsAdapted);
        Console.WriteLine("Restarts (High-Level): {0}", this.restarts);
        Console.WriteLine("Path-Max Boosts (High-Level): {0}", this.pathMaxBoosts);
        Console.WriteLine("Reverse Path-Max Boosts (High-Level): {0}", this.reversePathMaxBoosts);
        Console.WriteLine("Path-Max Plus Boosts (High-Level): {0}", this.pathMaxPlusBoosts);
        Console.WriteLine("Max Group Size (High-Level): {0}", this.maxSizeGroup);
        Console.WriteLine("Surplus Nodes Avoided (High-Level): {0}", this.surplusNodesAvoided);
        Console.WriteLine("MDD cache hits (High-Level): {0}", this.mddCacheHits);
        Console.WriteLine("Time planning paths (High-Level): {0}", this.timePlanningPaths);
        Console.WriteLine("Time building mdds (High-Level): {0}", this.timeBuildingMdds);

        output.Write(this.highLevelExpanded + Run.RESULTS_DELIMITER);
        output.Write(this.highLevelGenerated + Run.RESULTS_DELIMITER);
        output.Write(this.closedListHits + Run.RESULTS_DELIMITER);
        output.Write(this.partialExpansions + Run.RESULTS_DELIMITER);
        output.Write(this.bypasses + Run.RESULTS_DELIMITER);
        output.Write(this.nodesExpandedWithGoalCost + Run.RESULTS_DELIMITER);
        output.Write(this.bypassLookAheadNodesCreated + Run.RESULTS_DELIMITER);
        output.Write(this.cardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER);
        output.Write(this.conflictsBypassed + Run.RESULTS_DELIMITER);
        output.Write(this.cardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.semiCardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.nonCardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.mddsBuilt + Run.RESULTS_DELIMITER);
        output.Write(this.mddsAdapted + Run.RESULTS_DELIMITER);
        output.Write(this.restarts + Run.RESULTS_DELIMITER);
        output.Write(this.pathMaxBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.reversePathMaxBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.pathMaxPlusBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.maxSizeGroup + Run.RESULTS_DELIMITER);
        output.Write(this.surplusNodesAvoided + Run.RESULTS_DELIMITER);
        output.Write(this.mddCacheHits + Run.RESULTS_DELIMITER);
        output.Write(this.timePlanningPaths + Run.RESULTS_DELIMITER);
        output.Write(this.timeBuildingMdds + Run.RESULTS_DELIMITER);

        this.solver.OutputAccumulatedStatistics(output);
        if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
            this.singleAgentSolver.OutputAccumulatedStatistics(output);

        this.openList.OutputStatistics(output);
    }

    public virtual int NumStatsColumns
    {
        get
        {
            int numSolverStats = this.solver.NumStatsColumns;
            if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
                numSolverStats += this.singleAgentSolver.NumStatsColumns;
            return 23 + numSolverStats + this.openList.NumStatsColumns;
        }
    }

    public virtual void ClearStatistics()
    {
        this.solver.ClearAccumulatedStatistics(); // Is this correct? Or is it better not to do it?
        if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
            this.singleAgentSolver.ClearAccumulatedStatistics();
        this.ClearPrivateStatistics();
        this.openList.ClearStatistics();
    }

    public virtual void ClearAccumulatedStatistics()
    {
        this.accHLExpanded = 0;
        this.accHLGenerated = 0;
        this.accClosedListHits = 0;
        this.accPartialExpansions = 0;
        this.accBypasses = 0;
        this.accNodesExpandedWithGoalCost = 0;
        this.accBypassLookAheadNodesCreated = 0;
        this.accCardinalLookAheadNodesCreated = 0;
        this.accConflictsBypassed = 0;
        this.accCardinalConflictSplits = 0;
        this.accSemiCardinalConflictSplits = 0;
        this.accNonCardinalConflictSplits = 0;
        this.accMddsBuilt = 0;
        this.accMddsAdapted = 0;
        this.accRestarts = 0;
        this.accPathMaxBoosts = 0;
        this.accReversePathMaxBoosts = 0;
        this.accPathMaxPlusBoosts = 0;
        this.accMaxSizeGroup = 1;
        this.accSurplusNodesAvoided = 0;
        this.accMddCacheHits = 0;
        this.accTimePlanningPaths = 0;
        this.accTimeBuildingMdds = 0;

        this.solver.ClearAccumulatedStatistics();
        if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
            this.singleAgentSolver.ClearAccumulatedStatistics();

        this.openList.ClearAccumulatedStatistics();
    }

    public virtual void AccumulateStatistics()
    {
        this.accHLExpanded += this.highLevelExpanded;
        this.accHLGenerated += this.highLevelGenerated;
        this.accClosedListHits += this.closedListHits;
        this.accPartialExpansions += this.partialExpansions;
        this.accBypasses += this.bypasses;
        this.accNodesExpandedWithGoalCost += this.nodesExpandedWithGoalCost;
        this.accBypassLookAheadNodesCreated += this.bypassLookAheadNodesCreated;
        this.accCardinalLookAheadNodesCreated += this.cardinalLookAheadNodesCreated;
        this.accConflictsBypassed += this.conflictsBypassed;
        this.accCardinalConflictSplits += this.cardinalConflictSplits;
        this.accSemiCardinalConflictSplits += this.semiCardinalConflictSplits;
        this.accNonCardinalConflictSplits += this.nonCardinalConflictSplits;
        this.accMddsBuilt += this.mddsBuilt;
        this.accMddsAdapted += this.mddsAdapted;
        this.accRestarts += this.restarts;
        this.accPathMaxBoosts += this.pathMaxBoosts;
        this.accReversePathMaxBoosts += this.reversePathMaxBoosts;
        this.accPathMaxPlusBoosts += this.pathMaxPlusBoosts;
        this.accMaxSizeGroup = Math.Max(this.accMaxSizeGroup, this.maxSizeGroup);
        this.accSurplusNodesAvoided += this.surplusNodesAvoided;
        this.accMddCacheHits += this.mddCacheHits;
        this.accTimePlanningPaths += this.timePlanningPaths;
        this.accTimeBuildingMdds += this.timeBuildingMdds;

        // this.solver statistics are accumulated every time it's used.

        this.openList.AccumulateStatistics();
    }

    public virtual void OutputAccumulatedStatistics(TextWriter output)
    {
        Console.WriteLine("{0} Accumulated Expanded Nodes (High-Level): {1}", this, this.accHLExpanded);
        Console.WriteLine("{0} Accumulated Generated Nodes (High-Level): {1}", this, this.accHLGenerated);
        Console.WriteLine("{0} Accumulated Closed List Hits (High-Level): {1}", this, this.accClosedListHits);
        Console.WriteLine("{0} Accumulated Partial Expansions (High-Level): {1}", this, this.accPartialExpansions);
        Console.WriteLine("{0} Accumulated Adoptions (High-Level): {1}", this, this.accBypasses);
        Console.WriteLine("{0} Accumulated Nodes Expanded With Goal Cost (High-Level): {1}", this, this.accNodesExpandedWithGoalCost);
        Console.WriteLine("{0} Accumulated Look Ahead Nodes Created (High-Level): {1}", this, this.accNodesExpandedWithGoalCost);
        Console.WriteLine("{0} Accumulated Conflicts Bypassed With Adoption (High-Level): {1}", this, this.accConflictsBypassed);
        Console.WriteLine("{0} Accumulated Cardinal Conflicts Splits (High-Level): {1}", this, this.accCardinalConflictSplits);
        Console.WriteLine("{0} Accumulated Semi-Cardinal Conflicts Splits (High-Level): {1}", this, this.accSemiCardinalConflictSplits);
        Console.WriteLine("{0} Accumulated Non-Cardinal Conflicts Splits (High-Level): {1}", this, this.accNonCardinalConflictSplits);
        Console.WriteLine("{0} Accumulated MDDs Built (High-Level): {1}", this, this.accMddsBuilt);
        Console.WriteLine("{0} Accumulated MDDs adapted (High-Level): {1}", this, this.accMddsAdapted);
        Console.WriteLine("{0} Accumulated Restarts (High-Level): {1}", this, this.accRestarts);
        Console.WriteLine("{0} Accumulated Path-Max Boosts (High-Level): {1}", this, this.accPathMaxBoosts);
        Console.WriteLine("{0} Accumulated Reverse Path-Max Boosts (High-Level): {1}", this, this.accReversePathMaxBoosts);
        Console.WriteLine("{0} Accumulated Path-Max Plus Boosts (High-Level): {1}", this, this.accPathMaxPlusBoosts);
        Console.WriteLine("{0} Max Group Size (High-Level): {1}", this, this.accMaxSizeGroup);
        Console.WriteLine("{0} Accumulated Surplus Nodes Avoided (High-Level): {1}", this, this.accSurplusNodesAvoided);
        Console.WriteLine("{0} Accumulated MDD cache hits (High-Level): {1}", this, this.accMddCacheHits);
        Console.WriteLine("{0} Accumulated time planning paths (High-Level): {1}", this, this.accTimePlanningPaths);
        Console.WriteLine("{0} Accumulated time building MDDs (High-Level): {1}", this, this.accTimeBuildingMdds);

        output.Write(this.accHLExpanded + Run.RESULTS_DELIMITER);
        output.Write(this.accHLGenerated + Run.RESULTS_DELIMITER);
        output.Write(this.accClosedListHits + Run.RESULTS_DELIMITER);
        output.Write(this.accPartialExpansions + Run.RESULTS_DELIMITER);
        output.Write(this.accBypasses + Run.RESULTS_DELIMITER);
        output.Write(this.accNodesExpandedWithGoalCost + Run.RESULTS_DELIMITER);
        output.Write(this.accBypassLookAheadNodesCreated + Run.RESULTS_DELIMITER);
        output.Write(this.accCardinalLookAheadNodesCreated + Run.RESULTS_DELIMITER);
        output.Write(this.accConflictsBypassed + Run.RESULTS_DELIMITER);
        output.Write(this.accCardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.accSemiCardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.accNonCardinalConflictSplits + Run.RESULTS_DELIMITER);
        output.Write(this.accMddsBuilt + Run.RESULTS_DELIMITER);
        output.Write(this.accMddsAdapted + Run.RESULTS_DELIMITER);
        output.Write(this.accRestarts + Run.RESULTS_DELIMITER);
        output.Write(this.accPathMaxBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.accReversePathMaxBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.accPathMaxPlusBoosts + Run.RESULTS_DELIMITER);
        output.Write(this.accMaxSizeGroup + Run.RESULTS_DELIMITER);
        output.Write(this.accSurplusNodesAvoided + Run.RESULTS_DELIMITER);
        output.Write(this.accMddCacheHits + Run.RESULTS_DELIMITER);
        output.Write(this.accTimePlanningPaths + Run.RESULTS_DELIMITER);
        output.Write(this.accTimeBuildingMdds + Run.RESULTS_DELIMITER);

        this.solver.OutputAccumulatedStatistics(output);
        if (Object.ReferenceEquals(this.singleAgentSolver, this.solver) == false)
            this.singleAgentSolver.OutputAccumulatedStatistics(output);

        this.openList.OutputAccumulatedStatistics(output);
    }

    public bool debug = false;
    private bool equivalenceWasOn;

    /// <summary>
    /// 
    /// </summary>
    /// <returns></returns>
    protected void SetGlobals()
    {
        this.equivalenceWasOn = AgentState.EquivalenceOverDifferentTimes;
        AgentState.EquivalenceOverDifferentTimes = false;
    }

    protected void CleanGlobals()
    {
        AgentState.EquivalenceOverDifferentTimes = this.equivalenceWasOn;
    }

    public bool Solve()
    {
        //this.SetGlobals(); // Again, because we might be resuming a search that was stopped.

        int initialEstimate = 0;
        if (openList.Count > 0)
            initialEstimate = openList.Peek().f;

        int maxExpandedNodeF = -1;
        int currentCost = -1;

        while (openList.Count > 0)
        {
            // Check if max time has been exceeded
            if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME)
            {
                this.solutionCost = (int) Constants.SpecialCosts.TIMEOUT_COST;
                Console.WriteLine("Out of time");
                this.solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate.
                                                                         // Can't use top of OPEN's f-value instead of openList.Peek().f because we may have
                                                                         // timed out while expanding and before generating nodes that would have led to the
                                                                         // optimal solution, and the remaining nodes in OPEN are junk
                this.Clear(); // Total search time exceeded - we're not going to resume this search.
                this.CleanGlobals();
                return false;
            }

            Debug.WriteLine("Getting the next node from OPEN");
            CbsNode currentNode = openList.Remove();

            if (currentNode.f > this.maxSolutionCost)  // A late heuristic application may have increased the node's cost
            {
                continue;
                // Don't expand the node.
                // This will exhaust the open list, assuming Fs of nodes chosen for expansions
                // are monotonically increasing.
                // TODO: Pass maxSolutionCost to the DynamicLazyOpenList so it can inform the 
                //       heuristic not to try to improve its estimate to make the node's f more
                //       than 1 + maxSolutionCost.
                //       OTOH, DynamicLazyOpenList only tries to increase the cost by 1 over the
                //       current minimum, and it would never reach maxSolutionCost + 1 to try to
                //       go above it.
            }


            currentNode.ChooseConflict();  // Does nothing if this node has already been partially expanded before

            // Notice that even though we may discover cardinal conflicts in hindsight later,
            // there would be no point in pushing their node back at that point,
            // as we would've already made the split by then.

            Debug.WriteLine("Expanding node: (or returning its solution, if it's a goal node)");
            currentNode.DebugPrint();

            // Update nodesExpandedWithGoalCost statistic
            if (currentNode.g > currentCost) // Needs to be here because the goal may have a cost unseen before
            {
                currentCost = currentNode.g;
                this.nodesExpandedWithGoalCost = 0;
            }
            else if (currentNode.g == currentCost) // check needed because macbs node cost isn't exactly monotonous
            {
                this.nodesExpandedWithGoalCost++;
            }

            // Check if node is the goal
            if (currentNode.GoalTest())
            {
                Trace.Assert(currentNode.g >= maxExpandedNodeF,
                                $"CBS goal node found with lower cost than the max cost node ever expanded ({currentNode.g} < {maxExpandedNodeF})");
                // This is subtle, but MA-CBS may expand nodes in a non non-decreasing order:
                // If a non-optimal constraint is expanded upon and we decide to merge the agents,
                // the resulting node can have a lower cost than before, since we ignore the non-optimal constraint
                // because the conflict it addresses is between merged nodes.
                // The resulting lower-cost node will have other constraints, that will raise the cost of its children back to at least its original cost,
                // since the node with the non-optimal constraint was only expanded because its competitors that had an optimal
                // constraint to deal with the same conflict apparently found the other conflict that I promise will be found,
                // and so their cost was not smaller than this sub-optimal node.
                // To make MA-CBS costs non-decreasing, we can choose not to ignore constraints that deal with conflicts between merged nodes.
                // That way, the sub-optimal node will find a sub-optimal merged solution and get a high cost that will push it deep into the open list.
                // But the cost would be to create a possibly sub-optimal merged solution where an optimal solution could be found instead, and faster,
                // since constraints make the low-level heuristic perform worse.
                // For an example for this subtle case happening, see problem instance 63 of the random grid with 4 agents,
                // 55 grid cells and 9 obstacles.

                Debug.WriteLine("-----------------");
                this.solutionCost = currentNode.g;
                this.solutionDepth = this.solutionCost - initialEstimate;
                this.goalNode = currentNode; // Saves the single agent plans and costs
                // The joint plan is calculated on demand.
                this.Clear(); // Goal found - we're not going to resume this search
                this.CleanGlobals();
                return true;
            }

            if (maxExpandedNodeF < currentNode.f)
            {
                maxExpandedNodeF = currentNode.f;
                Debug.WriteLine("New max F: {0}", maxExpandedNodeF);
            }

            // Check conditions that stop the search before a goal is found
            if (currentNode.f >= this.targetF || // Node is good enough
                this.singleAgentSolver.GetAccumulatedGenerated() + this.solver.GetAccumulatedGenerated() > 
                    this.lowLevelGeneratedCap || // Stop because this is taking too long.
                                                    // We're looking at _generated_ low level nodes since that's an indication to the amount of work done,
                                                    // while expanded nodes is an indication of the amount of good work done.
                (this.milliCap != int.MaxValue && // (This check is much cheaper than the method call)
                    this.runner.ElapsedMilliseconds() > this.milliCap)) // Search is taking too long.
            {
                Debug.WriteLine("-----------------");
                this.solutionCost = maxExpandedNodeF; // This is the min possible cost so far.
                this.openList.Add(currentNode); // To be able to continue the search later
                this.CleanGlobals();
                return false;
            }

            // Expand
            bool wasUnexpandedNode = (currentNode.agentAExpansion == CbsNode.ExpansionState.NOT_EXPANDED &&
                                        currentNode.agentBExpansion == CbsNode.ExpansionState.NOT_EXPANDED);
            Expand(currentNode);
            if (wasUnexpandedNode)
                highLevelExpanded++;
            // Consider moving the following into Expand()
            if (currentNode.agentAExpansion == CbsNode.ExpansionState.EXPANDED &&
                currentNode.agentBExpansion == CbsNode.ExpansionState.EXPANDED) // Fully expanded
                currentNode.Clear();
        }

        // Check if max time has been exceeded
        if (this.runner.ElapsedMilliseconds() > Constants.MAX_TIME)
        {
            this.solutionCost = (int)Constants.SpecialCosts.TIMEOUT_COST;
            Console.WriteLine("Out of time");
            this.solutionDepth = maxExpandedNodeF - initialEstimate; // A minimum estimate
        }
        else
            this.solutionCost = (int) Constants.SpecialCosts.NO_SOLUTION_COST;
        this.Clear(); // we're not going to resume this search - it either timed out or the problem is unsolvable
        this.CleanGlobals();
        return false;
    }

    protected virtual bool ShouldMerge(CbsNode node)
    {
        return node.ShouldMerge(mergeThreshold, node.conflict.agentAIndex, node.conflict.agentBIndex);  //TODO: Add an option to use the old merge criterion
    }

    public virtual void Reset()
    {
        this.restarts++;
        this.openList.Clear();
        this.closedList.Clear();
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="node"></param>
    /// <param name="adopt"></param>
    /// <param name="children"></param>
    /// <param name="reinsertParent">If it was only partially expanded</param>
    /// <param name="adoptBy">If not given, adoption is done by expanded node</param>
    /// <returns>true if adopted - need to rerun this method, ignoring the returned children from this call, bacause adoption was performed</returns>
    protected (bool adopted, IList<CbsNode> children, bool reinsertParent) ExpandImpl(CbsNode node, bool adopt, CbsNode adoptBy = null)
    {
        CbsConflict conflict = node.GetConflict();
        var children = new List<CbsNode>();
        CbsNode child;
        int closedListHitChildCost;
        if (adoptBy == null)
            adoptBy = node;
        int adoptByH = adoptBy.h;

        bool leftSameCost = false; // To quiet the compiler
        bool rightSameCost = false;

        if (this.mergeThreshold != -1 && ShouldMerge(node))
        {
            if (this.mergeCausesRestart == false)
            {
                (child, closedListHitChildCost) = this.MergeExpand(node);
                if (child == null)
                    return (adopted: false, children, reinsertParent: false); // A timeout occured,
                                                                                // or the child was already in the closed list,
                                                                                // or there were just too many constraints
                                                                                // (happens with ID, which adds whole paths as constraints)
            }
            else
            {
                // TODO: What if planning a path for the merged agents finds a path with the same
                // cost as the sum of their current paths and no other conflicts exist? Should just
                // adopt this solution and get a goal node.
                // TODO: Save the cost of the group in a table, and use it as a heuristic in the future!
                child = new CbsNode(this.instance.agents.Length, this.solver,
                                    this.singleAgentSolver, this, node.agentsGroupAssignment);  // This will be the new root node
                child.MergeGroups(node.agentsGroupAssignment[conflict.agentAIndex], node.agentsGroupAssignment[conflict.agentBIndex],
                                  fixCounts: false  // This is a new root node, it doesn't have conflict counts yet
                                  );
                this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.GetGroupSize(conflict.agentAIndex));
                bool solved = child.Solve(this.minSolutionTimeStep);
                child.h = node.f - child.g;
                
                //if (this.debug)
                    Debug.WriteLine($"Restarting the search with agents {node.agentsGroupAssignment[conflict.agentAIndex]} and" +
                                    $" {node.agentsGroupAssignment[conflict.agentBIndex]} merged.");
                this.Reset();

                if (solved == false)  // Likely due to a time-out
                    return (adopted: false, children, reinsertParent: false);
            }
            // No need to try to adopt the child - there's only one so we're not branching.
            children.Add(child);
            return (adopted: false, children, reinsertParent: false);
        }

        bool reinsertParent = false;

        // Generate left child:
        (child, closedListHitChildCost) = ConstraintExpand(node, doLeftChild: true);
        if (child != null)
        {
            if (child == node) // Expansion deferred
                reinsertParent = true;
            else // New child
            {
                // First fit adoption: Greedily adopt the first child that's better than the parent
                if (adopt &&
                    AdoptConditionally(adoptBy, child, adoptByH))
                    return (adopted: true, children, reinsertParent);
                children.Add(child);
                leftSameCost = child.g == node.g;
                if (leftSameCost)
                {
                    Trace.Assert(node.conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.YES);
                }
                else
                {
                    Trace.Assert(node.conflict.willCostIncreaseForAgentA != CbsConflict.WillCostIncrease.NO);
                }
            }
        }
        else  // A timeout occured, or the child was already in the closed list.
        {
            if (closedListHitChildCost != -1)
                leftSameCost = closedListHitChildCost == node.g;
        }

        if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
            return (adopted: false, children, reinsertParent);

        // Generate right child:
        (child, closedListHitChildCost) = ConstraintExpand(node, doLeftChild: false);
        if (child != null)
        {
            if (child == node) // Expansion deferred
                reinsertParent = true;
            else // New child
            {
                // First fit adoption: Greedily adopt the first child that's better than the parent
                if (adopt &&
                    AdoptConditionally(adoptBy, child, adoptByH))
                    return (adopted: true, children, reinsertParent);
                children.Add(child);
                rightSameCost = child.g == node.g;
                if (rightSameCost)
                {
                    Trace.Assert(node.conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.YES);
                }
                else
                {
                    Trace.Assert(node.conflict.willCostIncreaseForAgentB != CbsConflict.WillCostIncrease.NO);
                }
            }
        }
        else  // A timeout occured, or the child was already in the closed list.
        {
            if (closedListHitChildCost != -1)
                rightSameCost = closedListHitChildCost == node.g;
        }

        if (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED || node.agentBExpansion == CbsNode.ExpansionState.DEFERRED)
            this.semiCardinalConflictSplits++; // Count only on the first expansion of the node. On the second expansion no count will be incremented.
        else
        {
            if (leftSameCost && rightSameCost)
                this.nonCardinalConflictSplits++;
            else if (leftSameCost || rightSameCost)
                this.semiCardinalConflictSplits++;
            else
                this.cardinalConflictSplits++;
        }

        return (adopted: false, children, reinsertParent);
    }
        
    private void ExpandIgnoringCardinalsButSupportingBP2(CbsNode node)
    {
        int parentCost = node.g;
        int parentH = node.h;
        IList<CbsNode> children = new List<CbsNode>(2);
        bool reinsertParent = false;
        bool adopted = false;
             
        int origCardinalConflictSplits = this.cardinalConflictSplits;
        int origSemiCardinalConflictSplits = this.semiCardinalConflictSplits;
        int origNonCardinalConflictSplits = this.nonCardinalConflictSplits;

        if (this.bypassStrategy == BypassStrategy.NONE)
        {
            (adopted, children, reinsertParent) = this.ExpandImpl(node, adopt: false, adoptBy: null);
        }
        else if (this.bypassStrategy == BypassStrategy.FIRST_FIT_LOOKAHEAD || node.parentAlreadyLookedAheadOf) // Do bypass but don't look ahead
        {
            bool adoptionPerformedBefore = false;
            while (true) // Until a node with a higher cost is found or a goal is found
            {
                var lookAheadOpenList = new OpenList<CbsNode>(this);
                var lookAheadSameCostNodes = new HashSet<CbsNode>();
                var lookAheadLargerCostNodes = new HashSet<CbsNode>();
                var lookAheadSameCostNodesToReinsertWithHigherCost = new HashSet<CbsNode>();
                lookAheadOpenList.Add(node);

                if (lookAheadOpenList.Count != 0)
                    Debug.WriteLine("Starting lookahead:");
                IList<CbsNode> lookAheadChildren;
                bool lookAheadReinsertParent;
                int expansions = 0;
                while (lookAheadOpenList.Count != 0)
                {
                    if (expansions + 1 > this.lookaheadMaxExpansions) // + 1 because we're checking before the coming expansion. lookaheadMaxExpansions = 1 is the minimum working value.
                    {
                        Debug.WriteLine("Lookahead count exceeded. Stopping lookahead.");
                        break;
                    }

                    if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                        return;

                    CbsNode lookAheadNode = lookAheadOpenList.Remove();
                    lookAheadNode.ChooseConflict();

                    Debug.WriteLine($"Looking ahead from node hash: {lookAheadNode.GetHashCode()}.");

                    (adopted, lookAheadChildren, lookAheadReinsertParent) = this.ExpandImpl(lookAheadNode, adopt: true, adoptBy: node);
                    expansions++;

                    if (adopted == true)
                    {
                        this.bypassLookAheadNodesCreated += lookAheadChildren.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise. +1 for the node itself before it adopted another solution.
                        this.cardinalConflictSplits = origCardinalConflictSplits;  // No split was done
                        this.semiCardinalConflictSplits = origSemiCardinalConflictSplits;
                        this.nonCardinalConflictSplits = origNonCardinalConflictSplits;
                        break; // Insert children into open list
                    }

                    if (lookAheadReinsertParent)
                        lookAheadSameCostNodesToReinsertWithHigherCost.Add(lookAheadNode);

                    foreach (var child in lookAheadChildren)
                    {
                        this.bypassLookAheadNodesCreated++;
                        this.closedList.Add(child, child); // Temporarily! Just so look ahead expansion can use this data
                        if (child.g == node.g)
                        {
                            lookAheadOpenList.Add(child);
                            lookAheadSameCostNodes.Add(child);
                        }
                        else
                            lookAheadLargerCostNodes.Add(child); // No need to check if it's already there since the closed list is properly maintained
                    }
                }

                if (adopted == false && lookAheadOpenList.Count == 0)
                    Debug.WriteLine("Lookahead exhausted all same cost nodes.");

                if (adopted && adoptionPerformedBefore == false)
                {
                    adoptionPerformedBefore = true;
                    this.bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed.
                }

                if (node.GoalTest()) // Lookahead found an admissible solution! (The original node to expand wasn't a goal)
                {
                    Debug.WriteLine("Goal found with same cost - stopping lookahead.");
                    this.openList.Add(node);
                    return;
                }

                if (adopted == false) // Insert children into open list
                {
                    if (node.h < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively
                        node.h = 1; // We know the goal isn't under it with the same cost

                    // We already expanded this whole subtree of same cost nodes (at least partially). Insert the frontier to the open list.
                    reinsertParent = false; // It may be reinserted as an element of the lookAheadSameCostNodesToReinsertWithHigherCost
                    children = lookAheadLargerCostNodes.ToList(); // Larger cost nodes aren't expanded so they're always on the frontier
                    int unexpandedSameCostNodes = lookAheadOpenList.Count;
                    while (lookAheadOpenList.Count != 0)
                    {
                        CbsNode child = lookAheadOpenList.Remove();
                        children.Add(child); // Unexapnded so it's also on the frontier
                        this.closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method
                    }
                    foreach (var reInsertNode in lookAheadSameCostNodesToReinsertWithHigherCost)
                    {
                        if (reInsertNode == node)
                        {
                            reinsertParent = true;
                            continue;
                        }
                        children.Add(reInsertNode);
                        this.closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method
                    }
                    foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes)
                        this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method
                    this.bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // None of these nodes weren't wasted effort, they were properly generated
                    this.highLevelGenerated += lookAheadSameCostNodes.Count
                                                - lookAheadSameCostNodesToReinsertWithHigherCost.Count
                                                - unexpandedSameCostNodes; // You could say they were generated and not looked ahead at, and they won't be counted later.
                    this.highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes;

                    break;
                }
                else // Adopted. Do a new round of expansions.
                {
                    foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes)
                        this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method
                    foreach (CbsNode lookAheadNode in lookAheadSameCostNodes)
                        this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method
                                                                // The only difference in closed list cleanup after adoption is that expanded same cost nodes are also removed from the closed list.
                }
            }
        }
        else // this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD and this set of costs not already done
        {
            // FIXME: lookaheadMaxExpansions isn't respected correctly here. We actually limit the number of same-cost nodes generated, which is similar but not the same.
            var lookAheadOpenList = new OpenList<CbsNode>(this);
            var lookAheadSameCostNodes = new HashSet<CbsNode>();
            var lookAheadLargerCostNodes = new HashSet<CbsNode>();
            var lookAheadSameCostNodesToReinsertWithHigherCost = new HashSet<CbsNode>();
            lookAheadOpenList.Add(node);
            node.parentAlreadyLookedAheadOf = true;

            if (lookAheadOpenList.Count != 0)
                Debug.WriteLine("Starting lookahead:");
            IList<CbsNode> lookAheadChildren;
            bool lookAheadReinsertParent;
            while (lookAheadOpenList.Count != 0)
            {
                if (lookAheadSameCostNodes.Count + 1 >= this.lookaheadMaxExpansions) // + 1 for the root
                {
                    Debug.WriteLine("Lookahead count exceeded. Stopping lookahead.");
                    break;
                }

                CbsNode lookAheadNode = lookAheadOpenList.Remove();
                lookAheadNode.ChooseConflict();

                if (runner.ElapsedMilliseconds() > Constants.MAX_TIME)
                    return;

                Debug.WriteLine($"Looking ahead from node hash: {lookAheadNode.GetHashCode()}.");

                (adopted, lookAheadChildren, lookAheadReinsertParent) = this.ExpandImpl(lookAheadNode, adopt: false);

                if (lookAheadReinsertParent)
                    lookAheadSameCostNodesToReinsertWithHigherCost.Add(lookAheadNode);

                foreach (var lookaheadChild in lookAheadChildren)
                {
                    this.bypassLookAheadNodesCreated++;
                    this.closedList.Add(lookaheadChild, lookaheadChild); // Temporarily! Just so look ahead expansion can use this data
                    if (lookaheadChild.g == node.g)
                    {
                        if (lookaheadChild.GoalTest()) // Lookahead found an admissible solution!
                        {
                            Debug.WriteLine("Goal found with same cost - stopping lookahead.");
                            this.openList.Add(lookaheadChild); // Technically should have just breaked and let node adopt child. This is just a short-cut.
                            this.bypasses++; // Just a technicality needed to make the adoption count not lower than if we didn't use immediate adoption. You could say we adopt the goal's solution.
                            return;
                        }

                        if (lookAheadSameCostNodes.Contains(lookaheadChild) == false)
                        {
                            lookAheadOpenList.Add(lookaheadChild);
                            lookAheadSameCostNodes.Add(lookaheadChild);
                        }
                    }
                    else
                        lookAheadLargerCostNodes.Add(lookaheadChild); // No need to check if it's already there :)
                }
            }

            if (node.h < 1 && lookAheadOpenList.Count == 0) // Looked ahead exhaustively
                node.h = 1; // We know the goal isn't under it with the same cost

            bool bypassPerformed = false;
            // Find the best look ahead node and check if it's worth adopting
            if (lookAheadSameCostNodes.Count != 0)
            {
                IList<CbsNode> lookAheadSameCostNodesSerialzed = lookAheadSameCostNodes.ToList<CbsNode>();
                CbsNode adoptionCandidate = lookAheadSameCostNodesSerialzed[0];
                for (int i = 1; i < lookAheadSameCostNodesSerialzed.Count; i++)
                {
                    if (lookAheadSameCostNodesSerialzed[i].TieBreak(adoptionCandidate) == -1)
                        adoptionCandidate = lookAheadSameCostNodesSerialzed[i];
                }
                if (AdoptConditionally(node, adoptionCandidate, parentH))
                {
                    bypassPerformed = true;
                    this.bypasses++;

                    this.cardinalConflictSplits = origCardinalConflictSplits;
                    this.semiCardinalConflictSplits = origSemiCardinalConflictSplits;
                    this.nonCardinalConflictSplits = origNonCardinalConflictSplits;

                    // Remove cancelled look-ahead nodes from closed list
                    foreach (CbsNode lookAheadNode in lookAheadSameCostNodes)
                        this.closedList.Remove(lookAheadNode);
                    foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes)
                        this.closedList.Remove(lookAheadNode);

                    if (this.openList.Count != 0 && this.openList.Peek().f < node.f)
                    // This is a new unexpanded node, and we just raised its h, so we can push it back
                    // FIXME: More duplication of the push back logic
                    {
                        reinsertParent = true;
                        children = new List<CbsNode>(); // Children will be generated when this node comes out of the open list

                        if (this.debug)
                            Debug.WriteLine("Reinserting node into the open list with h=1, since the goal wasn't found with the same cost under it.");
                    }
                    else
                    {
                        // We updated the node, need to re-expand it. Surprisingly, re-expansion can produce even better nodes for adoption, so we need to allow immediate expansion too.
                        // TODO: Just re-run the infinite lookahead? That would be the immediate adoption variant above
                        this.Expand(node);
                        return;
                    }
                }
            }

            if (bypassPerformed == false)
            {
                // Adoption not performed, and we already expanded this whole subtree of same cost nodes (at least partially)
                reinsertParent = false; // It may reinserted as an element of the children list
                children = lookAheadLargerCostNodes.ToList();
                int unexpandedSameCostNodes = lookAheadOpenList.Count;
                while (lookAheadOpenList.Count != 0)
                {
                    CbsNode child = lookAheadOpenList.Remove();
                    children.Add(child);
                    this.closedList.Remove(child); // Just so it'll be inserted into the open list at the end of the method
                }
                foreach (var reInsertNode in lookAheadSameCostNodesToReinsertWithHigherCost)
                {
                    if (reInsertNode == node)
                    {
                        reinsertParent = true;
                        continue;
                    }
                    children.Add(reInsertNode);
                    this.closedList.Remove(reInsertNode); // Just so it'll be inserted into the open list at the end of the method
                    if (reInsertNode == node) // Re-inserting the original node that was to be expanded, with a higher h
                        this.highLevelGenerated--; // It'll be counted as a new generated child later, and we don't need to count it twice
                }
                foreach (CbsNode lookAheadNode in lookAheadLargerCostNodes)
                    this.closedList.Remove(lookAheadNode); // Just so they'll be inserted into the open list at the end of the method
                this.bypassLookAheadNodesCreated -= lookAheadSameCostNodes.Count + lookAheadLargerCostNodes.Count; // These nodes weren't wasted effort
                this.highLevelGenerated += lookAheadSameCostNodes.Count - lookAheadSameCostNodesToReinsertWithHigherCost.Count - unexpandedSameCostNodes; // You could say they were generated and not looked ahead at, and they won't be counted later.
                this.highLevelExpanded += lookAheadSameCostNodes.Count - unexpandedSameCostNodes;
            }
        }

        // Both children considered. None adopted. Add them to the open list, and re-insert the partially expanded parent too if necessary.
        if (reinsertParent)
            this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts

        foreach (var child in children)
        {
            closedList.Add(child, child);

            // Bequeath remainder of h from parent
            int remainingParentH = parentH - (child.g - parentCost);
            if (child.h < remainingParentH)
                child.h = (ushort) remainingParentH;

            if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD)
            {
                if (child.g == parentCost) // Total cost didn't increase (yet)
                    child.parentAlreadyLookedAheadOf = true;
                else
                    child.parentAlreadyLookedAheadOf = false;
            }

            if (child.f <= this.maxSolutionCost)
            // Assuming h is an admissible heuristic, no need to generate nodes that won't get us
            // to the goal within the budget
            {
                this.highLevelGenerated++;
                this.addToGlobalConflictCount(child); // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict
                openList.Add(child);
            }
            else
            {
                this.surplusNodesAvoided++;
            }
        }
    }

    /// <summary>
    /// Supports Prefering cardinal conflicts and BP1 with lookaheadMaxExpansions == 1,
    /// assumes we choose cardinal conflicts using one of the methods
    /// </summary>
    /// <param name="node"></param>
    private void IcbsExpand(CbsNode node)
    {
        int parentCost = node.g;
        int parentH = node.h;
        IList<CbsNode> children = new List<CbsNode>(2);
        bool reinsertParent;
        bool adopted = false;

        int origCardinalConflictSplits = this.cardinalConflictSplits;
        int origSemiCardinalConflictSplits = this.semiCardinalConflictSplits;
        int origNonCardinalConflictSplits = this.nonCardinalConflictSplits;

        Trace.Assert(this.bypassStrategy == BypassStrategy.NONE || this.lookaheadMaxExpansions == 1, "Only BP1 is supported with cardinal conflict choice");  // Assumed in order to simplify code
        Trace.Assert(this.bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "Only first-fit adoption is supported with cardinal conflict choice"); // Assumed in order to simplify code

        bool adoptionPerformedBefore = false;

        // Adoption and cardinal-lookahead cycle
        while (true)
        {
            (adopted, children, reinsertParent) = this.ExpandImpl(node, adopt: bypassStrategy != BypassStrategy.NONE);

            if (this.mergeCausesRestart && (this.closedList.Count == 0)) // HACK: Means a restart was triggered
                break;

            if (adopted) // Either we found a goal, or we need to re-expand using a remaining conflict
            {
                if (adoptionPerformedBefore == false)
                {
                    adoptionPerformedBefore = true;
                    this.bypasses++; // Only count one adoption for the entire look ahead subtree branch traversed.
                }
                this.bypassLookAheadNodesCreated += children.Count + 1; // Either 1, if the first child was adopted, or 2 otherwise, +1 because the adopted child isn't returned.
                this.cardinalConflictSplits = origCardinalConflictSplits;
                this.semiCardinalConflictSplits = origSemiCardinalConflictSplits;
                this.nonCardinalConflictSplits = origNonCardinalConflictSplits;

                if (node.GoalTest()) // Adoption found an admissible solution! (The original node to expand wasn't a goal)
                {
                    Debug.WriteLine("Bypass found a goal! Inserting it into OPEN.");
                    this.openList.Add(node);
                    return;
                }

                Trace.Assert(node.conflict != null, "A conflict should have been found");
                // We might re-encounter conflicts we've already found aren't cardinal, but
                // since at least one path changed, the number of conflicts after adopting a child
                // might become smaller after adopting a child's solution this time
                continue;
            }

            if (
                children.All(child => child.g > node.g) && // All generated nodes have a higher cost
                ((node.agentAExpansion == CbsNode.ExpansionState.EXPANDED && node.agentBExpansion == CbsNode.ExpansionState.EXPANDED) || // Both children generated by now (possibly one in the past and one deferred to now)
                (node.agentAExpansion == CbsNode.ExpansionState.EXPANDED && node.agentBExpansion == CbsNode.ExpansionState.DEFERRED) || // One child still deferred, will surely have a higher cost (that's why it was deferred)
                (node.agentAExpansion == CbsNode.ExpansionState.DEFERRED && node.agentBExpansion == CbsNode.ExpansionState.EXPANDED)) // One child still deferred, will surely have a higher cost (that's why it was deferred)
                    ) // A cardinal conflict, or deferred expansion of a cardinal or semi-cardinal conflict.
            {
                if (this.debug)
                    Debug.WriteLine("This was a cardinal conflict, or deferred expansion of a cardinal or semi-cardinal conflict. Inserting the generated children into OPEN.");

                break; // Look no further
            }
            else
            {
                // This wasn't a cardinal conflict, continue cycling through conflicts
            }

            if (children.Any(child => child.GoalTest() && child.g == node.g)) // Admissable goal found (and adoption isn't enabled, otherwise the goal would have been adopted above)
            {
                if (this.debug)
                    Debug.WriteLine("Admissable goal found! Inserting the generated children into OPEN.");

                break; // Look no further
            }

            // This wasn't a cardinal conflict. Try to find a different conflict to try.
            if (node.ChooseNextPotentiallyCardinalConflicts() == false)
            {
                Debug.WriteLine("This was a non-cardinal conflict but there aren't more " +
                                "promising conflicts to try. Inserting the generated children into OPEN.");
                break; // Look no further
            }
            else
            {
                Debug.WriteLine($"This was a non-cardinal conflict. Trying potentially-cardinal conflict: {node.conflict}");
            }

            // Prepare to restart the loop
            // Cancel partial expansion effects:
            node.agentAExpansion = CbsNode.ExpansionState.NOT_EXPANDED;
            node.agentBExpansion = CbsNode.ExpansionState.NOT_EXPANDED;
            node.h = parentH;
            // Take care of counts:
            this.cardinalLookAheadNodesCreated += children.Count;
        }

        if (reinsertParent)
        {
            this.openList.Add(node); // Re-insert node into open list with higher cost, don't re-increment global conflict counts
        }
        else
        {
            node.ClearConflictChoiceData(); // We could do it even if there was a partial expansion and the node is
                                            // being re-inserted into OPEN.
                                            // If a lookahead chooser settled on
                                            // a semi-cardinal or non-cardinal, then there are no other conflicts left
                                            // so the conflict choosing state is unnecessary, but then ChooseNextConflict
                                            // would need to allow the iterator to be null.
                                            // If an MDD building chooser settled on a semi-cardinal or non-cardinal,
                                            // there could still be other conflicts, but they are known not to be cardinal.
        }

        // Path-Max stage
        // Reverse Path-Max (Mero, 1984) - operators aren't invertible, so we can only take min(h(Ci) + dist(P, Ci)) for h(P) (if it's higher)
        int minChildSumHAndOperatorCost = children.Max(child => child.h + (child.g - node.g));
        if (node.h < minChildSumHAndOperatorCost)
        {
            node.hBonus += minChildSumHAndOperatorCost - node.h;
            node.h = minChildSumHAndOperatorCost;
            ++reversePathMaxBoosts;
        }

        // Forward Path-Max (Mero, 1984)
        foreach (var child in children)
        {
            closedList.Add(child, child);

            // Bequeath remainder of h from parent
            int remainingParentH = parentH - (child.g - parentCost);
            if (child.h < remainingParentH)
            {
                child.h = (ushort)remainingParentH;
                this.pathMaxBoosts++;
            }
            // "Path-Max Plus"
            if (node.minimumVertexCover > 0 &&
                (this.conflictChoice == ConflictChoice.CARDINAL_MDD || this.conflictChoice == ConflictChoice.CARDINAL_LOOKAHEAD ||
                 this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_SMALLEST_GROUP ||
                 this.conflictChoice == ConflictChoice.CARDINAL_MDD_THEN_MERGE_EARLY_MOST_CONFLICTING_AND_SMALLEST_GROUP
                 ))
            {
                child.h = (ushort)Math.Max(child.h, node.minimumVertexCover - 1);  // -1 because we've just resolved a cardinal conflict.
                                                                                    // Even if the cost increased by more than 1 for the child, this is still our estimate, based on all the other conflicts
                this.pathMaxPlusBoosts++;
            }
            if (child.f <= this.maxSolutionCost)
            {
                this.highLevelGenerated++;
                this.addToGlobalConflictCount(child);  // TODO: Make MACBS_WholeTreeThreshold use nodes that do this automatically after choosing a conflict
                openList.Add(child);
            }
            else
                this.surplusNodesAvoided++;
        }
    }

    public virtual void Expand(CbsNode node)
    {
        if (this.bypassStrategy == BypassStrategy.BEST_FIT_LOOKAHEAD && node.parentAlreadyLookedAheadOf)
            Debug.WriteLine("Not looking ahead from this node, one of its ancestors of the same cost was already looked ahead of");

        if (this.conflictChoice == ConflictChoice.FIRST || this.conflictChoice == ConflictChoice.MOST_CONFLICTING_SMALLEST_AGENTS) // Then just choose a conflict once and stick with it.
        {
            this.ExpandIgnoringCardinalsButSupportingBP2(node);
        }
        else // We try to split according to cardinal conflicts
        {
            Trace.Assert(bypassStrategy != BypassStrategy.BEST_FIT_LOOKAHEAD, "For simplicity, BP2 is not supported when choosing cardinal conflicts");
            this.IcbsExpand(node);
        }
    }

    /// <summary>
    /// </summary>
    /// <param name="node"></param>
    /// <returns>A ValueTuple with:
    /// child: null if planning the child's path failed or it was a closed list hit, otherwise - the new child
    /// closedListHitChildCost: Used to tell if conflict is cardinal, semi-cardinal or
    /// non-cardinal when the child is a closed list hit.
    /// </returns>
    protected (CbsNode child, int closedListHitChildCost) MergeExpand(CbsNode node)
    {
        CbsConflict conflict = node.GetConflict();
        int closedListHitChildCost = -1;
            
        CbsNode child = new CbsNode(node, node.agentsGroupAssignment[conflict.agentAIndex], node.agentsGroupAssignment[conflict.agentBIndex]);
        if (closedList.ContainsKey(child) == false) // We may have already merged these agents in another node
        {
            Debug.WriteLine($"Merging agents {conflict.agentAIndex} and {conflict.agentBIndex}");
            int aCost = node.GetGroupCost(node.agentsGroupAssignment[conflict.agentAIndex]);
            int bCost = node.GetGroupCost(node.agentsGroupAssignment[conflict.agentBIndex]);
            int minNewCost;
            if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS)
                minNewCost = aCost + bCost;
            else if (Constants.costFunction == Constants.CostFunction.MAKESPAN ||
                        Constants.costFunction == Constants.CostFunction.MAKESPAN_THEN_SUM_OF_COSTS)
                minNewCost = Math.Max(aCost, bCost);
            else
                throw new NotImplementedException($"Unsupported cost function {Constants.costFunction}");
            if (this.useOldCost == false)
                minNewCost = -1;
            bool success = child.Replan(conflict.agentAIndex,  // or agentBIndex. Doesn't matter - they're in the same group.
                                        this.minSolutionTimeStep, minPathCost: minNewCost);

            if (success == false) // A timeout probably occured
                return (child: null, closedListHitChildCost);

            Debug.WriteLine($"Child hash: {child.GetHashCode()}");
            Debug.WriteLine($"Child cost: {child.g}");
            Debug.WriteLine($"Child min ops to solve: {child.minOpsToSolve}");
            Debug.WriteLine("");

            this.maxSizeGroup = Math.Max(this.maxSizeGroup, child.replanSize);

            return (child, closedListHitChildCost);
        }
        else
        {
            closedListHits++;
            return (child: null, closedListHitChildCost: closedList[child].g);
        }
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="node"></param>
    /// <param name="doLeftChild"></param>
    /// <returns>
    /// <returns>A ValueTuple with:
    /// child: null if planning the child's path failed or it was a closed list hit,
    /// the parent node if expansion was deferred, or otherwise - the new child.
    /// closedListHitChildCost: Used to tell if conflict is cardinal, semi-cardinal or
    /// non-cardinal when the child is a closed list hit.
    /// </returns>
    /// </returns>
    protected (CbsNode child, int closedListHitChildCost) ConstraintExpand(CbsNode node, bool doLeftChild)
    {
        CbsConflict conflict = node.GetConflict();
        int conflictingAgentIndex = doLeftChild? conflict.agentAIndex : conflict.agentBIndex;
        CbsNode.ExpansionState expansionsState = doLeftChild ? node.agentAExpansion : node.agentBExpansion;
        CbsNode.ExpansionState otherChildExpansionsState = doLeftChild ? node.agentBExpansion : node.agentAExpansion;
        string agentSide = doLeftChild? "left" : "right";
        int planSize = node.singleAgentPlans[conflictingAgentIndex].GetSize();  // Used to check if the conflict occurs while the agent is at its goal
        int groupSize = node.GetGroupSize(conflictingAgentIndex);
        CbsConflict.WillCostIncrease willCostIncrease = doLeftChild ? node.conflict.willCostIncreaseForAgentA : node.conflict.willCostIncreaseForAgentB;

        // Check if expansion should be deferred
        if (Constants.costFunction == Constants.CostFunction.SUM_OF_COSTS && // Otherwise adding a constraint to an agent at a time step after
                                                                                // it reaches its goal doesn't necessarily increase the cost,
                                                                                // so we're not allowed to defer expansion.
                                                                                // I think in a makespan variant this optimization is inapplicable:
                                                                                // You can't have a conflict at the last step of the agent with the longest plan,
                                                                                // because who would conflict with it? An agent with a longer plan? (no such agent)
                                                                                // An agent with a plan of the same length? (but goals don't collide)
            ((Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG &&
            expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.isVertexConflict == true &&  // An edge conflict at the goal isn't guaranteed
                                                                                                    // to increase the cost - the agent might be able to reach the goal from another direction
                conflict.timeStep >= node.singleAgentCosts[conflictingAgentIndex] && // Can't just check whether the node is at its goal - 
                                                                                        // the plan may involve it passing through its goal and returning to it later because of preexisting constraints.
                                                                                        // This assumes unit move costs
                node.h < conflict.timeStep + 1 - node.singleAgentCosts[conflictingAgentIndex] && // Otherwise we won't be increasing its h and there would be no reason to delay expansion
                                                                                                    // The agent's new cost will be at least conflict.timeStep + 1, so however much this is more than its current cost 
                                                                                                    // is an admissible heuristic
                groupSize == 1) || // Otherwise an agent in the group can be forced to take a longer
                                // route without increasing the group's cost because
                                // another agent would be able to take a shorter route.
            (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE &&
            expansionsState == CbsNode.ExpansionState.NOT_EXPANDED && conflict.isVertexConflict == true &&
            ((conflict.timeStep > planSize - 1 && node.h < 2) ||
                (conflict.timeStep == planSize - 1 && node.h < 1)) &&  // Otherwise we won't be increasing its h and there would be no reason to delay expansion
            groupSize == 1))) // Otherwise an agent in the group can be forced to take a longer
                                // route without increasing the group's cost because
                                // another agent would be able to take a shorter route.
        // Conflict happens when or after the agent reaches its goal, and the agent is in a single-agent group.
        // With multi-agent groups, banning the goal doesn't guarantee a higher cost solution,
        // since if an agent is forced to take a longer route it may enable another agent in the group
        // to take a shorter route, getting an alternative solution of the same cost
        // The child would cost a lot because:
        // A) All WAIT moves in the goal before leaving it now add to the g (if we're in the original problem variant).
        // B) We force the low level to compute a path longer than the optimal,
        //    and with a bad suprise towards the end in the form of a constraint,
        //    so the low-level's SIC heuristic performs poorly.
        // C) We're banning the GOAL from all directions (since this is a vertex conflict),
        //    so any alternative plan will at least cost 1 more.
        //    We're ignoring edge conflicts because they can only happen at the goal when reaching it,
        //    and aren't guaranteed to increase the cost because the goal can still be possibly reached from another edge.
        {
            // Defer expansion. The conflict happens while the agent is at its goal - the cost will
            // surely increase.
            if (otherChildExpansionsState == CbsNode.ExpansionState.DEFERRED)
                    throw new Exception("Unexpected: Expansion of both children deffered, " +
                        "but this is a vertex conflict so that means the targets for the " +
                        "two agents are equal, which is illegal");

            Debug.WriteLine($"Skipping {agentSide} child for now");
            if (doLeftChild)
                node.agentAExpansion = CbsNode.ExpansionState.DEFERRED;
            else
                node.agentBExpansion = CbsNode.ExpansionState.DEFERRED;
            // Add the minimal delta in the child's cost:
            // since we're banning the goal at conflict.timeStep, it must at least do conflict.timeStep+1 steps
            if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.ORIG)
            {
                node.h = Math.Max(node.h, (ushort)(conflict.timeStep + 1 - node.singleAgentCosts[conflictingAgentIndex]));
                // Technically, we've already made sure above we're going to increase the node's h,
                // This is just to make the line look correct without reading the complex if statement above.
            }
            else if (Constants.sumOfCostsVariant == Constants.SumOfCostsVariant.WAITING_AT_GOAL_ALWAYS_FREE)
            {
                if (conflict.timeStep > planSize - 1) // Agent will need to step out and step in to the goal, at least
                    node.h = Math.Max(node.h, (ushort) 1);
                else // Conflict is just when agent enters the goal, it'll have to at least wait one timestep.
                    node.h = Math.Max(node.h, (ushort) 1);
                // Technically, we've already made sure above we're going to increase the node's h,
                // This is just to make the line look correct without reading the complex if statement above.
            }
            this.partialExpansions++;  // The other child was surely generated, so we can count a partial expansion here.
            return (node, closedListHitChildCost: -1);
        }
        else if (expansionsState != CbsNode.ExpansionState.EXPANDED)
        // Agent expansion already skipped in the past or not forcing it from its goal - finally generate the child:
        {
            Debug.WriteLine($"Generating {agentSide} child");

            if (doLeftChild)
                node.agentAExpansion = CbsNode.ExpansionState.EXPANDED;
            else
                node.agentBExpansion = CbsNode.ExpansionState.EXPANDED;

            var newConstraint = new CbsConstraint(conflict, instance, doLeftChild);
            CbsNode child = new CbsNode(node, newConstraint, conflictingAgentIndex);

            if (this.doMalte && doLeftChild == false)
            {
                // Add the case where both agents shouldn't be at the conflict point to the _left_ child
                // by forcing the first agent in the _right_ child to _always_ be at the conflict point.
                // Notice that vertex conflicts create must constraint with NO_DIRECTION and edge conflicts
                // don't, as necessary.
                var newMustConstraint = new CbsConstraint(conflict, instance, true);
                child.SetMustConstraint(newMustConstraint);
            }

            if (closedList.ContainsKey(child) == false)
            {
                int oldCost = node.GetGroupCost(node.agentsGroupAssignment[conflictingAgentIndex]);
                int minNewCost = oldCost;
                if (willCostIncrease == CbsConflict.WillCostIncrease.YES)
                    minNewCost = oldCost + 1;

                int maxNewCost = int.MaxValue;
                if (willCostIncrease == CbsConflict.WillCostIncrease.NO)
                    maxNewCost = oldCost;

                if (this.useOldCost == false)
                {
                    minNewCost = -1;
                    maxNewCost = int.MaxValue;
                }

                bool success = child.Replan(conflictingAgentIndex, this.minSolutionTimeStep,  // The node takes the max between minSolutionTimeStep and the max time over all constraints.
                                            minPathCost: minNewCost, maxPathCost: maxNewCost);

                if (success == false)
                    return (null, closedListHitChildCost: -1); // A timeout probably occured

                Debug.WriteLine($"Child hash: {child.GetHashCode()}");
                Debug.WriteLine($"Child cost: {child.g}");
                Debug.WriteLine($"Child min ops to solve: {child.minOpsToSolve}");
                Debug.WriteLine($"Child num of agents that conflict: {child.totalInternalAgentsThatConflict}");
                Debug.WriteLine($"Child num of internal conflicts: {child.totalConflictsBetweenInternalAgents}");
                Debug.WriteLine("");

                if (child.g < node.g && groupSize == 1) // Catch the error early
                {
                    child.DebugPrint();
                    Debug.WriteLine("Child plan: (cost {0})", child.singleAgentCosts[conflictingAgentIndex]);
                    child.singleAgentPlans[conflictingAgentIndex].DebugPrint();
                    Debug.WriteLine("Parent plan: (cost {0})", node.singleAgentCosts[conflictingAgentIndex]);
                    node.singleAgentPlans[conflictingAgentIndex].DebugPrint();
                    Trace.Assert(false, $"Single agent node with lower cost than parent! {child.g} < {node.g}");
                }

                return (child, closedListHitChildCost: -1);
            }
            else
            {
                this.closedListHits++;
                Debug.WriteLine("Child already in closed list!");
                // No need to check if we need to reopen - reopening is only necessary if F is lower,
                // but for the same constraints and group assignment we're going to have the same
                // g, and the h in the closed list can only be higher (if it was already expanded
                // and the open list's expensive heuristic was computed for it).
                return (child: null, closedListHitChildCost: this.closedList[child].g);
            }
        }
        else
        {
            Debug.WriteLine("Child already generated before");
            return (child: null, closedListHitChildCost: -1);
        }
    }

    /// <summary>
    /// 
    /// </summary>
    /// <param name="node"></param>
    /// <param name="adoptionCandidate"></param>
    /// <param name="nodeOrigH"></param>
    /// <returns>Whether the candidate's solution was adopted</returns>
    protected bool AdoptConditionally(CbsNode node, CbsNode adoptionCandidate, int nodeOrigH)
    {
        Debug.WriteLine($"Considering adoption of node hash: {adoptionCandidate.GetHashCode()}.");

        if (adoptionCandidate.g == node.g && // No need to branch :)
            adoptionCandidate.TieBreak(node, ignorePartialExpansion: true, ignoreDepth: true) == -1
            )
        {
            this.conflictsBypassed += node.totalConflictsWithExternalAgents + node.totalConflictsBetweenInternalAgents
                                                    - adoptionCandidate.totalConflictsWithExternalAgents - adoptionCandidate.totalConflictsBetweenInternalAgents;
            node.AdoptSolutionOf(adoptionCandidate);
            node.h = nodeOrigH; // Cancel partial expansion h boost. This wasn't a cardinal conflict, so h could only have been from a partial expansion.
                                // FIXME: when semi-cardinal and non-cardinal conflicts start contributing to h, this won't be correct

            Debug.WriteLine("Child has same cost as parent and a better solution - child solution adopted by parent! (other generated children and partial expansions cancelled)");
            Debug.WriteLine("Node new details:");
            node.DebugPrint();
            return true;
        }
        Debug.WriteLine("Did not adopt.");
        Debug.WriteLine("");
        return false;
    }

    protected virtual void addToGlobalConflictCount(CbsConflict conflict) { }

    protected virtual void addToGlobalConflictCount(CbsNode node) { }

    public virtual Plan GetPlan()
    {
        if (this.solution == null)
            this.solution = this.goalNode.CalculateJointPlan();
        return this.solution;
    }

    public int GetSolutionDepth() { return this.solutionDepth; }
        
    public long GetMemoryUsed() { return Process.GetCurrentProcess().VirtualMemorySize64; }
        
    public SinglePlan[] GetSinglePlans()
    {
        return goalNode.singleAgentPlans;
    }

    public virtual int[] GetSingleCosts()
    {
        return goalNode.singleAgentCosts;
    }

    public int GetHighLevelExpanded() { return highLevelExpanded; }
    public int GetHighLevelGenerated() { return highLevelGenerated; }
    public int GetLowLevelExpanded() { return this.solver.GetAccumulatedExpanded(); }
    public int GetLowLevelGenerated() { return this.solver.GetAccumulatedGenerated(); }
    public int GetExpanded() { return highLevelExpanded; }
    public int GetGenerated() { return highLevelGenerated; }
    public int GetAccumulatedExpanded() { return accHLExpanded; }
    public int GetAccumulatedGenerated() { return accHLGenerated; }
    public int GetMaxGroupSize() { return this.maxSizeGroup; }
}

/// <summary>
/// Merges agents if they conflict more times than the given threshold in all the CT.
/// </summary>
public class MACBS_WholeTreeThreshold : CBS
{
    /// <summary>
    /// Counts conflicts between pairs of individual agents. To get counts for meta-agents, sum the values for their concrete agents.
    /// </summary>
    public int[][] globalConflictsCounter;

    public MACBS_WholeTreeThreshold(ICbsSolver singleAgentSolver, ICbsSolver generalSolver,
                                int mergeThreshold = -1,
                                BypassStrategy bypassStrategy = BypassStrategy.NONE,
                                bool doMalte = false,
                                ConflictChoice conflictChoice = ConflictChoice.FIRST,
                                ILazyHeuristic<CbsNode> heuristic = null,
                                bool disableTieBreakingByMinOpsEstimate = false,
                                int lookaheadMaxExpansions = 1,
                                bool mergeCausesRestart = false)
        : base(singleAgentSolver, generalSolver, mergeThreshold,
                bypassStrategy, doMalte, conflictChoice, heuristic,
                disableTieBreakingByMinOpsEstimate, lookaheadMaxExpansions,
                mergeCausesRestart)
    {
        //throw new NotImplementedException("Not supported until we decide how to count conflicts. Used to rely on the specific conflict chosen in each node.");
    }

    /// <summary>
    /// Implement ISolver.Setup.
    /// Assumes agent nums start from 0 and are consecutive.
    /// </summary>
    /// <param name="problemInstance"></param>
    /// <param name="runner"></param>
    public override void Setup(ProblemInstance problemInstance, Run runner)
    {
        this.MakeConflictMatrix(problemInstance);
        base.Setup(problemInstance, runner);
    }

    private void MakeConflictMatrix(ProblemInstance problemInstance)
    {
        this.globalConflictsCounter = new int[problemInstance.agents.Length][];
        for (int i = 0; i < globalConflictsCounter.Length; i++)
        {
            this.globalConflictsCounter[i] = new int[i];
            for (int j = 0; j < i; j++)
            {
                this.globalConflictsCounter[i][j] = 0;
            }
        }
    }

    protected override bool ShouldMerge(CbsNode node)
    {
        return node.ShouldMerge(mergeThreshold, globalConflictsCounter, node.conflict.agentAIndex, node.conflict.agentBIndex);  //TODO: Add an option to choose the old merge criterion
    }

    /// <summary>
    /// Old logic. Only add the chosen conflict to the counts
    /// </summary>
    /// <param name="conflict"></param>
    protected override void addToGlobalConflictCount(CbsConflict conflict)
    {
        if (conflict != null)
            globalConflictsCounter[Math.Max(conflict.agentAIndex, conflict.agentBIndex)][Math.Min(conflict.agentAIndex, conflict.agentBIndex)]++;
    }

    protected override void addToGlobalConflictCount(CbsNode node)
    {
        if (this.mergeCausesRestart == false)
        {
            // Old logic:
            if (node.conflict != null)
                globalConflictsCounter[Math.Max(node.conflict.agentAIndex, node.conflict.agentBIndex)][Math.Min(node.conflict.agentAIndex, node.conflict.agentBIndex)]++;
            // This only looks at conflicts between individual agents - doesn't take into account merges.
            // Merges are local when merge & restart is off so the global counts might be incorrect.
            // FIXME: Currently only looks at the chosen conflict.
        }
        else
        {
            //TODO: Add an option to use the old logic above here too
            for (int i = 0; i < this.GetProblemInstance().GetNumOfAgents(); i++)
            {
                if (node.newPlans[i] == false)
                    continue;
                foreach (var kvp in node.conflictCountsPerAgent[i])
                {
                    if (i > kvp.Key)
                        continue;
                    int groupRepA = node.agentsGroupAssignment[i];
                    int groupRepB = node.agentsGroupAssignment[kvp.Key];
                    if (groupRepA > groupRepB)
                        globalConflictsCounter[groupRepA][groupRepB] += kvp.Value;
                    else
                        globalConflictsCounter[groupRepB][groupRepA] += kvp.Value;
                }
            }
        }
    }

    public override string GetName()
    {
        string baseName = base.GetName();
        return baseName.Replace("Local", "Global");
    }

    public override void Reset()
    {
        base.Reset();
        //this.MakeConflictMatrix(this.instance);
    }
}