/**
 * 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: other != null.
     */
    public BodyQueue(BodyQueue other) {
        this.capacity = other.capacity;
        this.head = other.size();
        this.tail = 0;
        this.queue = new Body[this.capacity];
        for (int i = 0, j = other.tail; i < this.head; i++, j++) {
            this.queue[i] = other.queue[j % other.capacity];
        }
    }

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

    /**
     * Retrieves and removes the head of this queue, or returns 'null'
     * if this queue is empty.
     */
    public Body poll() {
        if (tail == head) {
            tail = 0;
            head = 0;
            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];
        head = size();
        for (int i = 0, j = tail; i < head; i++, j++) {
            tmp[i] = queue[j % capacity];
        }
        tail = 0;
        capacity *= 2;
        queue = tmp;
    }
}