/// <summary>
 /// Join the conflicting groups into a single group
 /// </summary>
 /// <param name="conflict">An object that describes the conflict</param>
 /// <returns>The composite group of agents</returns>
 protected virtual AgentsGroup JoinGroups(Conflict conflict)
 {
     return(conflict.group1.Join(conflict.group2));
 }
        /// <summary>
        /// Search for an optimal solution using the Independence Detection algorithm in Standley's paper,
        /// which utilises a CAT.
        /// </summary>
        /// <param name="runner"></param>
        /// <returns></returns>
        public bool ImprovedID(Run runner)
        {
            while (true)
            {
                Conflict conflict = FindConflictingGroups();
                // If there are no conflicts - can finish the run
                if (conflict == null)
                {
                    break;
                }

                // Try to solve the current conflict by replanning one of the groups
                if (this.allConflicts.Contains(conflict) == false)
                {
                    // Add to all conflicts to prevent trying to replan this conflict again
                    this.allConflicts.Add(conflict);

                    // Add plan of group2 to illegal moves table and replan group1 with equal cost
                    if ((conflict.timeOfConflict < conflict.group1.GetPlan().GetSize() - 1) ||
                        (conflict.group1.allAgentsState.Length > 1)) // Otherwise the conflict is while a single agent is at its goal, no chance of an alternate path with the same cost that avoids the conflict
                    {
                        conflict.group1.removeGroupFromCAT(conflictAvoidance);
                        bool resolved = conflict.group1.ReplanUnderConstraints(conflict.group2.GetPlan(), runner);
                        conflict.group1.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
                        if (resolved == true)
                        {
                            continue;
                        }
                    }
                    // Add plan of group1 to illegal moves table and replan group2 with equal cost
                    if ((conflict.timeOfConflict < conflict.group2.GetPlan().GetSize() - 1) ||
                        (conflict.group2.allAgentsState.Length > 1))
                    {
                        conflict.group2.removeGroupFromCAT(conflictAvoidance);
                        bool resolved = conflict.group2.ReplanUnderConstraints(conflict.group1.GetPlan(), runner);
                        conflict.group2.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
                        if (resolved == true)
                        {
                            continue;
                        }
                    }
                }

                // Groups are conflicting - need to join them to a single group
                allGroups.Remove(conflict.group1);
                allGroups.Remove(conflict.group2);
                // Remove both groups from avoidance table
                conflict.group1.removeGroupFromCAT(conflictAvoidance);
                conflict.group2.removeGroupFromCAT(conflictAvoidance);
                if (this.debug)
                {
                    Debug.WriteLine("Merging " + conflict);
                }
                AgentsGroup compositeGroup = this.JoinGroups(conflict);

                compositeGroup.instance.parameters[CONFLICT_AVOIDANCE] = conflictAvoidance;

                // Solve composite group with A*
                bool solved = compositeGroup.Solve(runner);

                if (compositeGroup.solutionCost > maxSolutionCostFound)
                {
                    maxSolutionCostFound = compositeGroup.solutionCost;
                }

                //add group to conflict avoidance table
                compositeGroup.addGroupToCAT(conflictAvoidance, maxSolutionCostFound);
                allGroups.AddFirst(compositeGroup);

                this.expanded  += compositeGroup.expanded;
                this.generated += compositeGroup.generated;

                if (compositeGroup.allAgentsState.Length > this.maxGroupSize)
                {
                    this.maxGroupSize = compositeGroup.allAgentsState.Length;
                }

                if (solved == false)
                {
                    this.totalCost = compositeGroup.solutionCost; // Should be some error code from Constants
                    return(false);
                }
            }
            return(true);
        }