An enhanced ArrayDeque enabling indexed access and modification

MyArrayDeque.java

import java.util.*;
import java.util.function.Consumer;

/**
 * An enhanced ArrayDeque enabling indexed access and modification via {@code get(int index)} and {@code set(int index, E e)} methods.
 * The majority of this implementation is directly borrowed from the official ArrayDeque.
 *
 * @param <E> the type of elements held in this collection
 */
public class MyArrayDeque<E> extends AbstractCollection<E> implements Deque<E> {

   private Object[] elements;

   private int head;
   private int tail;

   private static final int MIN_INITIAL_CAPACITY = 8;

   private static int calculateSize(int numElements) {
      int initialCapacity = MIN_INITIAL_CAPACITY;
      // Find the best power of two to hold elements.
      // Tests "<=" because arrays aren't kept full.
      if (numElements >= initialCapacity) {
         initialCapacity = numElements;
         initialCapacity |= (initialCapacity >>> 1);
         initialCapacity |= (initialCapacity >>> 2);
         initialCapacity |= (initialCapacity >>> 4);
         initialCapacity |= (initialCapacity >>> 8);
         initialCapacity |= (initialCapacity >>> 16);
         initialCapacity++;

         if (initialCapacity < 0)   // Too many elements, must back off
            initialCapacity >>>= 1;// Good luck allocating 2 ^ 30 elements
      }
      return initialCapacity;
   }

   private void allocateElements(int numElements) {
      elements = new Object[calculateSize(numElements)];
   }

   private void doubleCapacity() {
      assert head == tail;
      int p = head;
      int n = elements.length;
      int r = n - p; // number of elements to the right of p
      int newCapacity = n << 1;
      if (newCapacity < 0)
         throw new IllegalStateException("Sorry, deque too big");
      Object[] a = new Object[newCapacity];
      System.arraycopy(elements, p, a, 0, r);
      System.arraycopy(elements, 0, a, r, p);
      elements = a;
      head = 0;
      tail = n;
   }

   private <T> T[] copyElements(T[] a) {
      if (head < tail) {
         System.arraycopy(elements, head, a, 0, size());
      } else if (head > tail) {
         int headPortionLen = elements.length - head;
         System.arraycopy(elements, head, a, 0, headPortionLen);
         System.arraycopy(elements, 0, a, headPortionLen, tail);
      }
      return a;
   }

   public MyArrayDeque() {
      elements = new Object[16];
   }


   public MyArrayDeque(int numElements) {
      allocateElements(numElements);
   }


   public MyArrayDeque(Collection<? extends E> c) {
      allocateElements(c.size());
      addAll(c);
   }

   @SuppressWarnings("unchecked")
   public E get(int index) {
      if (index >= this.size() || index < 0) {
         throw new IndexOutOfBoundsException();
      }
      return (E) elements[(index + head) & (elements.length - 1)];
   }

   @SuppressWarnings("unchecked")
   public E set(int index, E e) {
      if (e == null)
         throw new NullPointerException();
      if (index >= this.size() || index < 0) {
         throw new IndexOutOfBoundsException();
      }
      E oldValue = (E) elements[(index + head) & (elements.length - 1)];
      elements[(index + head) & (elements.length - 1)] = e;
      return oldValue;
   }

   @Override
   public void addFirst(E e) {
      if (e == null)
         throw new NullPointerException();
      elements[head = (head - 1) & (elements.length - 1)] = e;
      if (head == tail)
         doubleCapacity();
   }

   @Override
   public void addLast(E e) {
      if (e == null)
         throw new NullPointerException();
      elements[tail] = e;
      if ((tail = (tail + 1) & (elements.length - 1)) == head)
         doubleCapacity();
   }

   @Override
   public boolean offerFirst(E e) {
      addFirst(e);
      return true;
   }

   @Override
   public boolean offerLast(E e) {
      addLast(e);
      return true;
   }

   @Override
   public E removeFirst() {
      E x = pollFirst();
      if (x == null)
         throw new NoSuchElementException();
      return x;
   }

   @Override
   public E removeLast() {
      E x = pollLast();
      if (x == null)
         throw new NoSuchElementException();
      return x;
   }

   @Override
   public E pollFirst() {
      int h = head;
      @SuppressWarnings("unchecked")
      E result = (E) elements[h];
      // Element is null if deque empty
      if (result == null)
         return null;
      elements[h] = null;     // Must null out slot
      head = (h + 1) & (elements.length - 1);
      return result;
   }

   @Override
   public E pollLast() {
      int t = (tail - 1) & (elements.length - 1);
      @SuppressWarnings("unchecked")
      E result = (E) elements[t];
      if (result == null)
         return null;
      elements[t] = null;
      tail = t;
      return result;
   }

   @Override
   public E getFirst() {
      @SuppressWarnings("unchecked")
      E result = (E) elements[head];
      if (result == null)
         throw new NoSuchElementException();
      return result;
   }

   @Override
   public E getLast() {
      @SuppressWarnings("unchecked")
      E result = (E) elements[(tail - 1) & (elements.length - 1)];
      if (result == null)
         throw new NoSuchElementException();
      return result;
   }

   @SuppressWarnings("unchecked")
   @Override
   public E peekFirst() {
      // elements[head] is null if deque empty
      return (E) elements[head];
   }

   @SuppressWarnings("unchecked")
   @Override
   public E peekLast() {
      return (E) elements[(tail - 1) & (elements.length - 1)];
   }


   @Override
   public boolean removeFirstOccurrence(Object o) {
      if (o == null)
         return false;
      int mask = elements.length - 1;
      int i = head;
      Object x;
      while ((x = elements[i]) != null) {
         if (o.equals(x)) {
            delete(i);
            return true;
         }
         i = (i + 1) & mask;
      }
      return false;
   }

   @Override
   public boolean removeLastOccurrence(Object o) {
      if (o == null)
         return false;
      int mask = elements.length - 1;
      int i = (tail - 1) & mask;
      Object x;
      while ((x = elements[i]) != null) {
         if (o.equals(x)) {
            delete(i);
            return true;
         }
         i = (i - 1) & mask;
      }
      return false;
   }

   @Override
   public boolean add(E e) {
      addLast(e);
      return true;
   }

   @Override
   public boolean offer(E e) {
      return offerLast(e);
   }

   @Override
   public E remove() {
      return removeFirst();
   }

   @Override
   public E poll() {
      return pollFirst();
   }

   @Override
   public E element() {
      return getFirst();
   }

   @Override
   public E peek() {
      return peekFirst();
   }

   @Override
   public void push(E e) {
      addFirst(e);
   }


   @Override
   public E pop() {
      return removeFirst();
   }

   @Override
   public boolean remove(Object o) {
      return removeFirstOccurrence(o);
   }


   @Override
   public void clear() {
      int h = head;
      int t = tail;
      if (h != t) { // clear all cells
         head = tail = 0;
         int i = h;
         int mask = elements.length - 1;
         do {
            elements[i] = null;
            i = (i + 1) & mask;
         } while (i != t);
      }
   }

   @Override
   public boolean contains(Object o) {
      if (o == null)
         return false;
      int mask = elements.length - 1;
      int i = head;
      Object x;
      while ((x = elements[i]) != null) {
         if (o.equals(x))
            return true;
         i = (i + 1) & mask;
      }
      return false;
   }

   @Override
   public int size() {
      return (tail - head) & (elements.length - 1);
   }


   @Override
   public boolean isEmpty() {
      return head == tail;
   }

   private void checkInvariants() {
      assert elements[tail] == null;
      assert head == tail ? elements[head] == null :
            (elements[head] != null &&
                  elements[(tail - 1) & (elements.length - 1)] != null);
      assert elements[(head - 1) & (elements.length - 1)] == null;
   }

   private boolean delete(int i) {
      checkInvariants();
      final Object[] elements = this.elements;
      final int mask = elements.length - 1;
      final int h = head;
      final int t = tail;
      final int front = (i - h) & mask;
      final int back = (t - i) & mask;

      // Invariant: head <= i < tail mod circularity
      if (front >= ((t - h) & mask))
         throw new ConcurrentModificationException();

      // Optimize for least element motion
      if (front < back) {
         if (h <= i) {
            System.arraycopy(elements, h, elements, h + 1, front);
         } else { // Wrap around
            System.arraycopy(elements, 0, elements, 1, i);
            elements[0] = elements[mask];
            System.arraycopy(elements, h, elements, h + 1, mask - h);
         }
         elements[h] = null;
         head = (h + 1) & mask;
         return false;
      } else {
         if (i < t) { // Copy the null tail as well
            System.arraycopy(elements, i + 1, elements, i, back);
            tail = t - 1;
         } else { // Wrap around
            System.arraycopy(elements, i + 1, elements, i, mask - i);
            elements[mask] = elements[0];
            System.arraycopy(elements, 1, elements, 0, t);
            tail = (t - 1) & mask;
         }
         return true;
      }
   }


   @Override
   public Object[] toArray() {
      return copyElements(new Object[size()]);
   }

   @SuppressWarnings("unchecked")
   @Override
   public <T> T[] toArray(T[] a) {
      int size = size();
      if (a.length < size)
         a = (T[]) java.lang.reflect.Array.newInstance(
               a.getClass().getComponentType(), size);
      copyElements(a);
      if (a.length > size)
         a[size] = null;
      return a;
   }

   @Override
   public Iterator<E> iterator() {
      return new DeqIterator();
   }

   @Override
   public Iterator<E> descendingIterator() {
      return new DescendingIterator();
   }

   private class DeqIterator implements Iterator<E> {
      /**
       * Index of element to be returned by subsequent call to next.
       */
      private int cursor = head;

      /**
       * Tail recorded at construction (also in remove), to stop
       * iterator and also to check for comodification.
       */
      private int fence = tail;

      /**
       * Index of element returned by most recent call to next.
       * Reset to -1 if element is deleted by a call to remove.
       */
      private int lastRet = -1;

      public boolean hasNext() {
         return cursor != fence;
      }

      public E next() {
         if (cursor == fence)
            throw new NoSuchElementException();
         @SuppressWarnings("unchecked")
         E result = (E) elements[cursor];
         // This check doesn't catch all possible comodifications,
         // but does catch the ones that corrupt traversal
         if (tail != fence || result == null)
            throw new ConcurrentModificationException();
         lastRet = cursor;
         cursor = (cursor + 1) & (elements.length - 1);
         return result;
      }

      public void remove() {
         if (lastRet < 0)
            throw new IllegalStateException();
         if (delete(lastRet)) { // if left-shifted, undo increment in next()
            cursor = (cursor - 1) & (elements.length - 1);
            fence = tail;
         }
         lastRet = -1;
      }

      public void forEachRemaining(Consumer<? super E> action) {
         Objects.requireNonNull(action);
         Object[] a = elements;
         int m = a.length - 1, f = fence, i = cursor;
         cursor = f;
         while (i != f) {
            @SuppressWarnings("unchecked") E e = (E) a[i];
            i = (i + 1) & m;
            if (e == null)
               throw new ConcurrentModificationException();
            action.accept(e);
         }
      }
   }

   private class DescendingIterator implements Iterator<E> {
      /*
       * This class is nearly a mirror-image of DeqIterator, using
       * tail instead of head for initial cursor, and head instead of
       * tail for fence.
       */
      private int cursor = tail;
      private int fence = head;
      private int lastRet = -1;

      public boolean hasNext() {
         return cursor != fence;
      }

      public E next() {
         if (cursor == fence)
            throw new NoSuchElementException();
         cursor = (cursor - 1) & (elements.length - 1);
         @SuppressWarnings("unchecked")
         E result = (E) elements[cursor];
         if (head != fence || result == null)
            throw new ConcurrentModificationException();
         lastRet = cursor;
         return result;
      }

      public void remove() {
         if (lastRet < 0)
            throw new IllegalStateException();
         if (!delete(lastRet)) {
            cursor = (cursor + 1) & (elements.length - 1);
            fence = head;
         }
         lastRet = -1;
      }
   }

}