public class PriorityQueue<TElement, TPriority>
{
    /// <summary>
    /// 保存所有節(jié)點的一維數(shù)組且每一項是個元組
    /// </summary>
    private (TElement Element, TPriority Priority)[] _nodes;

    /// <summary>
    /// 優(yōu)先級比較器，這里用的泛型，比較器可以自己實現(xiàn)
    /// </summary>
    private readonly IComparer<TPriority>? _comparer;

    /// <summary>
    /// 當前堆的大小
    /// </summary>
    private int _size;

    /// <summary>
    /// 版本號
    /// </summary>
    private int _version;

    /// <summary>
    /// 代表父節(jié)點最多有4個子節(jié)點，也就是d=4(d=4時好像效率最高)
    /// </summary>
    private const int Arity = 4;

    /// <summary>
    /// 使用位運算符，表示左移2或右移2(效率更高)，即相當于除以4，
    /// </summary>
    private const int Log2Arity = 2;

    /// <summary>
    ///  構(gòu)造函數(shù)初始化堆和比較器
    /// </summary>
    public PriorityQueue()
    {
        _nodes = Array.Empty<(TElement, TPriority)>();
        _comparer = InitializeComparer(null);
    }

    /// <summary>
    ///  重載構(gòu)造函數(shù)，來定義比較器否則使用默認的比較器
    /// </param>
    public PriorityQueue(IComparer<TPriority>? comparer)
    {
        _nodes = Array.Empty<(TElement, TPriority)>();
        _comparer = InitializeComparer(comparer);
    }
    private static IComparer<TPriority>? InitializeComparer(IComparer<TPriority>? comparer)
    {
        //如果是值類型，如果是默認比較器則返回null
        if (typeof(TPriority).IsValueType)
        {
            if (comparer == Comparer<TPriority>.Default)
            {
                return null;
            }

            return comparer;
        }
        //否則就使用自定義的比較器
        else
        {
            return comparer ?? Comparer<TPriority>.Default;
        }
    }

    /// <summary>
    /// 獲取索引的父節(jié)點
    /// </summary>
    private int GetParentIndex(int index) => (index - 1) >> Log2Arity;

    /// <summary>
    /// 獲取索引的左子節(jié)點
    /// </summary>
    private int GetFirstChildIndex(int index) => (index << Log2Arity) + 1;
}

public void Enqueue(TElement element, TPriority priority)
{
    //拿到最大位置的索引，然后再將數(shù)組長度+1
    int currentSize = _size++;
    _version++;
    //如果長度相等，說明已經(jīng)到達最大位置，數(shù)組需要擴容了才能容下更多的元素
    if (_nodes.Length == currentSize)
    {
        //擴容,參數(shù)是代表數(shù)組最小容量
        Grow(currentSize + 1);
    }

    if (_comparer == null)
    {
        
        MoveUpDefaultComparer((element, priority), currentSize);
    }
    else
    {
        MoveUpCustomComparer((element, priority), currentSize);
    }
}
private void Grow(int minCapacity)
{
    //增長倍數(shù)
    const int GrowFactor = 2;
    //每次擴容的最小值
    const int MinimumGrow = 4;
    //每次擴容都2倍擴容
    int newcapacity = GrowFactor * _nodes.Length;

    //數(shù)組不能大于最大長度
    if ((uint)newcapacity > Array.MaxLength) newcapacity = Array.MaxLength;

    //使用他們兩個的最大值
    newcapacity = Math.Max(newcapacity, _nodes.Length + MinimumGrow);

    //如果比參數(shù)小，則使用參數(shù)的最小值
    if (newcapacity < minCapacity) newcapacity = minCapacity;
    //重新分配內(nèi)存，設(shè)置大小，因為數(shù)組的保存在內(nèi)存中是連續(xù)的
    Array.Resize(ref _nodes, newcapacity);
}
private void MoveUpDefaultComparer((TElement Element, TPriority Priority) node, int nodeIndex)
{
    //nodes保存副本
    (TElement Element, TPriority Priority)[] nodes = _nodes;
    //這里入隊操作是從空閑索引第一個位置開始插入
    while (nodeIndex > 0)
    {
        //找父節(jié)點索引位置
        int parentIndex = GetParentIndex(nodeIndex);
        (TElement Element, TPriority Priority) parent = nodes[parentIndex];
        //插入節(jié)點和父節(jié)點比較，如果小于0(默認比較器情況下構(gòu)建的小頂堆)，則交換位置
        if (Comparer<TPriority>.Default.Compare(node.Priority, parent.Priority) < 0)
        {
            nodes[nodeIndex] = parent;
            nodeIndex = parentIndex;
        }
        //算是性能優(yōu)化吧，不必檢查所有節(jié)點，當發(fā)現(xiàn)大于時，就直接退出就可以了
        else
        {
            break;
        }
    }
    //將插入節(jié)點放到它應(yīng)該的位置
    nodes[nodeIndex] = node;
}

public TElement Dequeue()
{
    if (_size == 0)
    {
        throw new InvalidOperationException(SR.InvalidOperation_EmptyQueue);
    }
    //將堆頂元素返回
    TElement element = _nodes[0].Element;
    //然后調(diào)整堆
    RemoveRootNode();
    return element;
}
private void RemoveRootNode()
{
    //記錄最后一個元素的索引位置，并且將堆的大小-1
    int lastNodeIndex = --_size;
    _version++;

    if (lastNodeIndex > 0)
    {
        //堆的大小已經(jīng)被減1，所以將最后一個元素作為副本，開始從堆頂重新整理堆
        (TElement Element, TPriority Priority) lastNode = _nodes[lastNodeIndex];
        if (_comparer == null)
        {
            MoveDownDefaultComparer(lastNode, 0);
        }
        else
        {
            MoveDownCustomComparer(lastNode, 0);
        }
    }

    if (RuntimeHelpers.IsReferenceOrContainsReferences<(TElement, TPriority)>())
    {
        //將最后一個位置的元素設(shè)置為默認值
        _nodes[lastNodeIndex] = default;
    }
}
private void MoveDownDefaultComparer((TElement Element, TPriority Priority) node, int nodeIndex)
{
    (TElement Element, TPriority Priority)[] nodes = _nodes;
    //堆的實際大小
    int size = _size;

    int i;
    //當左子節(jié)點的索引小于堆的實際大小時
    while ((i = GetFirstChildIndex(nodeIndex)) < size)
    {
        //左子節(jié)點元素
        (TElement Element, TPriority Priority) minChild = nodes[i];
        //當前左子節(jié)點的索引
        int minChildIndex = i;
        //這里即找到所有同一個父節(jié)點的相鄰子節(jié)點，但是要判斷是否超出了總的大小
        int childIndexUpperBound = Math.Min(i + Arity, size);
        //按照索引區(qū)間順序查找，并根據(jù)比較器找到最小的子元素位置
        while (++i < childIndexUpperBound)
        {
            (TElement Element, TPriority Priority) nextChild = nodes[i];
            if (Comparer<TPriority>.Default.Compare(nextChild.Priority, minChild.Priority) < 0)
            {
                minChild = nextChild;
                minChildIndex = i;
            }
        }
        //如果最后一個節(jié)點的元素，比這個最小的元素還小，那么直接放到父節(jié)點即可
        if (Comparer<TPriority>.Default.Compare(node.Priority, minChild.Priority) <= 0)
        {
            break;
        }
        //將最小的子元素賦值給父節(jié)點
        nodes[nodeIndex] = minChild;
        nodeIndex = minChildIndex;
    }
    //將最后一個節(jié)點放到空閑出來的索引位置
    nodes[nodeIndex] = node;
}