public class BinaryHeap
    {
        protected HeapItem array;
        protected int heapSize;
        public BinaryHeap(int size)
        {
            heapSize = -1;
            array = new HeapItem;
        }
       
        public Object RetriveMaximum()
        {
            return array.obj;
        }
        public Object ExtractMin()
        {
            
            HeapItem max = array;
            array = array;
            Heapify(0);
            return max.obj;
        }
        public void RemoveMin()
        {
            array = array;
            Heapify(0);
        }
        public void HeapInsert(HeapItem item)
        {
            heapSize++;
            int i = heapSize;
            while(i>=1 && array<item)
            {
                array = array;
                i = Parent(i);
            }
            array = item;
        }
        protected void Heapify(int i)
        {
            int r = Right(i);
            int l = Left(i);
            int min;
            if (l < this.heapSize && array >= array)
                min = l;
            else
                min = i;
            if (r < this.heapSize && array >= array)
                min = r;
            if(min!=i)
            {
                #region exchange A with A
                HeapItem temp = array;
                array = array;
                array = temp;
                #endregion
                Heapify(min);
            }
        }

        protected int Parent(int i)
        {
            return(i - 1) / 2;
        }
        protected int Right(int i)
        {
            return 2 * i + 2;
        }
        protected int Left(int i)
        {
            return 2 * i + 1;
        }
    }

public Stack<Node> AstarPathSearching(Node start,Node end,Heuristic heuristic)
        {
            //Empty openSet and closeSet set
            openSet.Clear();
            closeSet.Clear();
            //Initialize start node
            start.CostSoFar = 0;
            start.EstimateCost = heuristic.Estimate(start);
            start.Father = null;
            //Add start node to openSet
            openSet.Add(start, start.TotalEstimateCost);
            Node currentNode;
            while (openSet.IsEmpty() == false)
            {
                //Extract the min Node(by TotalEstimateCost) in the openSet
                currentNode = (Node)openSet.Remove();
                
                //if the currentNode is the goal than, we have the shortest path!
                if (currentNode.Row == end.Row && currentNode.Col == end.Col)
                    return GetPath(end);
                //add currentNode to closeSet
                closeSet.Add(currentNode);
                int tentative_g_score;
                bool tentative_is_better;
                foreach(Node connection in connections)
                {
                    if (closeSet.Contains(connection))
                        continue;
                    tentative_g_score = currentNode.CostSoFar + 1;
                    if (openSet.Contains(connection) == false)
                    {
                        openSet.Add(connection, connection.TotalEstimateCost);
                        tentative_is_better = true;
                    }
                    else
                    {
                        if (tentative_g_score < connection.CostSoFar)
                            tentative_is_better = true;
                        else
                            tentative_is_better = false;
                    }
                    if(tentative_is_better)
                    {
                        connection.Father = currentNode;
                        connection.CostSoFar = tentative_g_score;
                        connection.EstimateCost = heuristic.Estimate(connection);
                    }
                }
            }
            return null;
        }