public class HeapQueue{

    private final Integer[] items;
    private int index = 0;
    private int _capacity;

    public HeapQueue(int capacity) {
        items = new Integer[capacity];
        _capacity = capacity;
    }

    public void enqueue(Integer item) {
        items[index] = item;
        int currentPos = index;

        while (items[currentPos].compareTo(items[(currentPos - 1) / 2]) > 0)
        {
            Integer tmpData = items[currentPos];
            items[currentPos] = items[(currentPos - 1) / 2];
            items[(currentPos - 1) / 2] = tmpData;
            currentPos = (currentPos - 1) / 2;
        }
        index++;
    }

    public Integer dequeue() {
        if(index == 0)
            throw new IllegalArgumentException("Cannot dequeue from an empty queue");

        Integer passangerToReturn = items[0];

        items[0] = items[index-1];
        items[index-1] = null;
        int currentPos = 0;
        boolean foundRightPosition = false;

        while (!foundRightPosition && items[currentPos] != null)
        {
            int leftPos = (currentPos * 2) + 1;
            int rightPos = (currentPos+1) * 2;
            //Get left value
            Integer left;
            if(leftPos < _capacity)
                left = items[leftPos];
            else
                left = null;
            //Get right value
            Integer right;
            if(rightPos < _capacity)
                right= items[rightPos];
            else
                right = null;

            if(left != null && left.compareTo(right)>=0 && items[currentPos].compareTo(left) < 0)
            {
                Integer tmp = left;
                items[leftPos] = items[currentPos];
                items[currentPos] = tmp;
                currentPos = leftPos;
            }
            else if(right!=null && right.compareTo(left)>=0 && items[currentPos].compareTo(right)<0)
            {
                Integer tmp = right;
                items[rightPos] = items[currentPos];
                items[currentPos] = tmp;
                currentPos = rightPos;
            }
            else
            {
                foundRightPosition = true;
            }
        }

        index--;
        return passangerToReturn;
    }


    public void printQueue()
    {
        for(Integer p : items)
            if(p!=null)
                System.out.println(p);
        System.out.println("\n");
    }

    public static void main(String[] args){
        HeapQueue hq = new HeapQueue(10);
        hq.printQueue();

        hq.enqueue(5);
        hq.enqueue(8);
        hq.enqueue(2);
        hq.enqueue(3);
        hq.enqueue(7);
        hq.enqueue(4);
        hq.enqueue(9);
        hq.enqueue(1);
        hq.printQueue();

        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());
        System.out.println(hq.dequeue());

    }
}