/**
 * A queue of bodies. A collection designed for holding bodies prior to processing.
 * The bodies of the queue can be accessed in a FIFO (first-in-first-out) manner,
 * i.e., the body that was first inserted by 'add' is retrieved first by 'poll'.
 * The number of elements of the queue is not limited.
 */
public class BodyQueue {
    private int capacity;
    private int head = 0;
    private int tail = 0;
    private Body[] queue;

    public BodyQueue() {
        this(4);
    }

    /**
     * Initializes this queue with an initial capacity.
     * Precondition: initialCapacity > 0.
     */
    public BodyQueue(int initialCapacity) {
        this.capacity = initialCapacity;
        this.queue = new Body[this.capacity];
    }

    /**
     * Initializes this queue as an independent copy of the specified queue.
     * Calling methods of this queue will not affect the specified queue
     * and vice versa.
     * Precondition: q != null.
     */
    public BodyQueue(BodyQueue q) {
        this.capacity = q.capacity;
        this.head = q.size();
        this.tail = 0;
        this.queue = new Body[this.capacity];
        for (int i = 0; i < q.size(); i++) {
            this.queue[i] = q.queue[i];
        }
    }

    /**
     * Adds the specified body 'b' to this queue.
     */
    public void add(Body b) {
        queue[head] = b;
        head = (head + 1) % capacity;
        if (head == tail) {
            doubleCapacity();
            head = capacity / 2;
        }
    }

    /**
     * Retrieves and removes the head of this queue, or returns 'null'
     * if this queue is empty.
     */
    public Body poll() {
        if (tail == head) {
            return null;
        }
        Body b = queue[tail];
        queue[tail] = null;
        tail = (tail + 1) % capacity;
        return b;
    }

    /**
     * Returns the number of bodies in this queue.
     */
    public int size() {
        return (head - tail + capacity) % capacity;
    }

    /**
     * Double the capacity of the queue.
     */
    private void doubleCapacity() {
        Body[] tmp = new Body[capacity * 2];
        for (int i = head, j = 0; i < tail + capacity; i++, j++) {
            tmp[j] = queue[i % capacity];
        }
        head = capacity;
        tail = 0;
        capacity *= 2;
        queue = tmp;
    }
}