import java.util.Collection;
import java.util.AbstractQueue;
import java.util.Iterator;

import java.util.NoSuchElementException;
import java.util.ConcurrentModificationException;

import static java.lang.Math.max;

/**
 * A maximum priority queue based on an unordered array.
 * Supports fast constant-time insertions of the elements, but
 * the retrieve-max-value methods work only in linear time.
 */
class MaxPQUnorderedArray<T extends Comparable<? super T>>
        extends AbstractQueue<T> {
    // we will start with an array with 2 empty cells
    public static final int MIN_SIZE = 2;

    private T[] data; 
    private int size = 0;
    private int nQueueMutations = 0;

    public MaxPQUnorderedArray() {
        this(MIN_SIZE);
    }
    
    /**
     * Creates a MaxPQ containing the elements from the
     * specified collection.
     */
    @SuppressWarnings("unchecked")
    public MaxPQUnorderedArray(Collection<T> elements) {
        final int arraySize = elements.size()
            + max(MIN_SIZE, elements.size() / 2);
        data = (T[]) new Comparable<?>[arraySize];

        int i = 0;
        for (T element : elements) {
            data[i] = element;
            i++;
        }
        this.size = elements.size();
    }
    
    /**
     * Create an empty queue with the specified initial capacity.
     * @param   initialCapacity has to be at least as large as
     *          {@code MIN_SIZE}.
     */
    @SuppressWarnings("unchecked")
    public MaxPQUnorderedArray(int initialCapacity) {
        if (initialCapacity < MIN_SIZE) {
            throw new IllegalStateException(String.format(
                "Expected: initialCapacity >= MIN_SIZE."
                + " Got: initialCapacity = %d", initialCapacity
            ));
        }
        data = (T[]) new Comparable<?>[MIN_SIZE];
    }
    
    @SuppressWarnings("unchecked")
    private MaxPQUnorderedArray(T[] data, int size) {
        T[] newData = (T[]) new Comparable<?>[data.length];
        System.arraycopy(data, 0, newData, 0, data.length);
        this.size = size;
        this.data = newData;
    }
    
    /**
     * Adds an element to the queue.
     * @return true in case the queue is modified as a result of the call.
     */
    @Override
    public boolean offer(T value) {
        grow(1);
        data[size] = value;
        size++;
        nQueueMutations++;
        return true;
    }
    
    /**
     * Adds all of the elements from the specified collection.
     * @return  true in case every element from the collection has been
     *          added to the queue.
     */
    @Override
    public boolean addAll(Collection<? extends T> elements) {
        grow(elements.size());
        int i = 0;
        for (T element : elements) {
            data[size + i] = element;
            i++;
        }
        size += elements.size();
        nQueueMutations++;
        return true;
    }
    
    /**
     * Retrieves, but does not remove, the max value stored in the queue
     * or returns {@code null} in case the queue is empty.
     * This method can be slightly improved by keeping the position
     * of the max element, so that subsequential calls of {@code element} 
     * or {@code remove} would take constant time instead of linear.
     */
    @Override
    public T peek() {
        if (size == 0) {
            return null;
        }
        T max = data[0];
        for (int i = 1; i < size; i++) {
            if (max.compareTo(data[i]) < 0) {
                max = data[i];
            }
        }
        return max;
    }

    /**
     * Retrieves and removes the maximum element of the queue or
     * just retunrls {@code null} in case the queue is empty.
     */
    @Override
    public T poll() {
        if (size == 0) {
            return null;
        }
        int iMax = 0;
        for (int i = 1; i < size; i++) {
            if (data[iMax].compareTo(data[i]) < 0) {
                iMax = i;
            }
        }
        T max = data[iMax];
        swap(data, iMax, size - 1);
        data[size - 1] = null;
        this.size--;
        shrink();
        nQueueMutations++;
        return max;
    }

    /**
     * Removes all elements from the queue.
     */
    @Override
    @SuppressWarnings("unchecked")
    public void clear() {
        size = 0;
        data = (T[]) new Comparable<?>[MIN_SIZE];
        nQueueMutations++;
    }
    
    @Override
    public boolean isEmpty() {
        return size == 0;
    }
    
    @Override
    public int size() {
        return size;
    }

    @Override
    public PQIterator iterator() {
        return new PQIterator();
    }
    
    /**
     * Reallocates the memory used to store the elements of the queue
     * in a way to fit next `by` added elements. Grows the length of the
     * array at least 1.5 times. We call this method beforehand the
     * actual insertions of the elements, because it allows us to
     * have fully filled array.
     */
    private void grow(int by) {
        if (size + by > data.length) {
            // keep the length of the array at least MIN_SIZE elements
            // ahead of the number of actual elements stored in the queue
            reallocate(size + by + max(MIN_SIZE, (size + by) / 2));
        }
    }
    
    /**
     * Reallocates the memory used to store the elements of the queue
     * in a way that the array would be at most 25% full.
     */
    private void shrink() {
        if (size == 0) {
            reallocate(MIN_SIZE);
        // the length of the array will be shortened at least twice,
        // so to keep the min-size-invariant the length of the array
        // should be at least of length 2 * MIN_SIZE
        } else if (data.length > 2 * MIN_SIZE
                && size <= data.length / 4) {
            // make the array that would be 50% full
            reallocate(size + max(MIN_SIZE, size));
        }
    }
    
    /**
     * Changes the size of the array in which the elements are stored.
     */
    @SuppressWarnings("unchecked")
    private void reallocate(int newSize) {
        assert newSize >= size();

        Comparable<?>[] newData = new Comparable<?>[newSize];
        System.arraycopy(data, 0, newData, 0, size());
        data = (T[]) newData;
    }

    private class PQIterator implements Iterator<T> {
        private final MaxPQUnorderedArray<T> pqCopy;
        private int iLastlyTraversed = -1;
        private int[] initialIndices;

        private final int nInitialQueueMutations;

        public PQIterator() {
            pqCopy = new MaxPQUnorderedArray<T>(data, size);
            initialIndices = new int[data.length];
            for (int i = 0; i < data.length; i++) {
                initialIndices[i] = i;
            }
            nInitialQueueMutations = nQueueMutations;
        }

        public boolean hasNext() {
            if (nInitialQueueMutations != nQueueMutations) {
                throw new ConcurrentModificationException(
                    "The queue has been modified."
                );
            }
            return !pqCopy.isEmpty();
        }

        public T next() {
            if (nInitialQueueMutations != nQueueMutations) {
                throw new ConcurrentModificationException(
                    "The queue has been modified."
                );
            }
            if (!hasNext()) {
                throw new NoSuchElementException("The queue is empty");
            }
            int iMax = 0;
            for (int i = 1; i < pqCopy.size; i++) {
                if (pqCopy.data[iMax].compareTo(pqCopy.data[i]) < 0) {
                    iMax = i;
                }
            }
            // when an element is removed from the pqCopy, it is
            // swapped with the last element of the array, so
            // we should also swap the indices
            iLastlyTraversed = initialIndices[iMax];
            initialIndices[iMax] = initialIndices[pqCopy.size - 1];
            initialIndices[pqCopy.size - 1] = -1;
            
            return pqCopy.remove();
        }

        public void remove() {
            if (nInitialQueueMutations != nQueueMutations) {
                throw new ConcurrentModificationException(
                    "The queue has been modified."
                );
            }
            if (iLastlyTraversed == -1) {
                throw new IllegalStateException();
            }
            if (iLastlyTraversed != size - 1) {
                // indices to the right from the removed elements
                // should be shifted
                for (int i = 0; i < pqCopy.size; i++) {
                    if (initialIndices[i] > iLastlyTraversed) {
                        initialIndices[i]--;
                    }
                }
                System.arraycopy(data, iLastlyTraversed + 1, data,
                        iLastlyTraversed, size - iLastlyTraversed - 1);
            }
            data[size - 1] = null;
            size--;
            iLastlyTraversed = -1;
        }
    }

    private static void swap(Object[] array, int i, int j) {
        assert rangeCheck(array, i) && rangeCheck(array, j);
        
        if (i != j) {
            Object temp = array[i];
            array[i] = array[j];
            array[j] = temp;
        }
    }

    private static boolean rangeCheck(Object[] array, int i) {
        return array != null && i >= 0 && i < array.length;
    }

    public static void main(String[] args) {
        MaxPQUnorderedArray<Integer> pq = new MaxPQUnorderedArray<>();
        for (int i = 0; i < 10; i++) {
            pq.add(i);
        }
        Iterator<Integer> iterator = pq.iterator();
        while (iterator.hasNext()) {
            int next = iterator.next();
            System.out.printf("%d ", next);
            if (next == 5 || next == 2 || next == 3) {
                iterator.remove();
            }
        }
        
        System.out.println();
        for (int i : pq) {
            System.out.printf("%d ", i);
        }
    }
}