ParingQueueTest

Search.cs

    public static class Search
    {
        private struct StatePriorityPair<TState, TCost>
        {
            public StatePriorityPair(TState state, TCost cost)
            {
                State = state;
                Cost = cost;
            }

            public TState State { get; }
            public TCost Cost { get; }

            internal class Comparer : IComparer<StatePriorityPair<TState, TCost>>
            {
                public Comparer(IComparer<TCost> costComparer)
                {
                    CostComparer = costComparer ?? throw new ArgumentNullException(nameof(costComparer));
                }

                private IComparer<TCost> CostComparer { get; }

                public int Compare(StatePriorityPair<TState, TCost> x, StatePriorityPair<TState, TCost> y)
                {
                    return CostComparer.Compare(x.Cost, y.Cost);
                }
            }
        }

        private struct Ignore
        {
        }

        public static IEnumerable<TState> Expand<TState, TCost, TDisciminator>(IEnumerable<TState> starts, Func<TState, IEnumerable<TState>> move, Func<TState, TDisciminator> discriminatorFunction, Func<TState, TCost> costFunction, IEqualityComparer<TDisciminator>? discriminatorEquivalence, IComparer<TCost>? costOrdering) where TDisciminator : notnull
        {
            if (starts is null)
                throw new ArgumentNullException(nameof(starts));

            if (move is null)
                throw new ArgumentNullException(nameof(move));

            if (discriminatorFunction is null)
                throw new ArgumentNullException(nameof(discriminatorFunction));

            if (costFunction is null)
                throw new ArgumentNullException(nameof(costFunction));

            costOrdering ??= Comparer<TCost>.Default;
            discriminatorEquivalence ??= EqualityComparer<TDisciminator>.Default;

            var due = new AlgoKit.Collections.Heaps.PairingHeap<StatePriorityPair<TState, TCost>, Ignore>(new StatePriorityPair<TState, TCost>.Comparer(costOrdering));
            var index = new Dictionary<TDisciminator, AlgoKit.Collections.Heaps.PairingHeapNode<StatePriorityPair<TState, TCost>, Ignore>>(discriminatorEquivalence);

            void consider(TState candidate)
            {
                var disciminator = discriminatorFunction(candidate);
                var cost = costFunction(candidate);

                if (index.TryGetValue(disciminator, out var found))
                {
                    if (costOrdering.Compare(cost, found.Key.Cost) < 0)
                        due.Update(found, new StatePriorityPair<TState, TCost>(candidate, cost));
                }
                else
                {
                    index.Add(disciminator, due.Add(new StatePriorityPair<TState, TCost>(candidate, cost), default));
                }
            }

            foreach (var start in starts)
            {
                consider(start);
            }

            while (!due.IsEmpty)
            {
                var next = due.Pop();
                yield return next.Key.State;

                foreach (var candidate in move(next.Key.State))
                {
                    consider(candidate);
                }
            }
        }
    }