Algorithms

class NodeHeap
{
  private Node[] m_nodeHeap = new Node[1000000];
  private uint m_lastNodeIndex;

  public long Count
  {
    get { return m_nodeHeap.LongLength - 1; }
  }

  public Node NodeAt(uint index)
  {
    return m_nodeHeap[index];
  }

  public void RemoveAt(uint index)
  {
    if (index > m_lastNodeIndex)
    {
      throw new ArgumentOutOfRangeException("uint index");
    }

    SwapNode(ref m_nodeHeap[index], ref  m_nodeHeap[m_lastNodeIndex]);

    m_nodeHeap[m_lastNodeIndex--] = null;

    SiftUp(index);
  }

  public uint IndexOf(Node n) //this needs optimized
  {
    for (uint i = 1; i <= m_lastNodeIndex; i++)
    {
      if (m_nodeHeap[i] == n) return i;
    }

    return 0;
  }

  public void Add(Node n)
  {
    m_nodeHeap[++m_lastNodeIndex] = n;

    SiftDown(m_lastNodeIndex);
  }

  public void SiftUp(uint index)
  {
    if (index >= m_lastNodeIndex) return;

    uint next = index;

    next *= 2;

    if (next > m_lastNodeIndex) return;

    if (m_nodeHeap[next].Cost < m_nodeHeap[index].Cost)
    {
      if (next != m_lastNodeIndex && m_nodeHeap[next + 1].Cost < m_nodeHeap[next].Cost)
      {
        SwapNode(ref m_nodeHeap[++next], ref m_nodeHeap[index]);
      }
      else
      {
        SwapNode(ref m_nodeHeap[next], ref m_nodeHeap[index]);
      }

      SiftUp(next);
    }
  }

  public void SiftDown(uint index)
  {
    if (index == 1) return;

    uint next = index;

    next /= 2;

    if (m_nodeHeap[next].Cost > m_nodeHeap[index].Cost)
    {
      SwapNode(ref m_nodeHeap[next], ref m_nodeHeap[index]);
      SiftDown(next);
    }
  }

  public Node RemoveFirst()
  {
    Node n = m_nodeHeap[1];
    RemoveAt(1);
    return n;
  }

  private void SwapNode(ref Node a, ref Node b)
  {
    Node temp = a;
    a = b;
    b = temp;
  }
}