/**
 * A map that associates a body with a force exerted on it. The number of
 * key-value pairs is not limited.
 */
public class BodyForceMap {
    private int size = 0;
    private int capacity;
    private Body[] keys;
    private Vector3[] values;

    public BodyForceMap() {
        this(4);
    }

    /**
     * Initializes this map with an initial capacity.
     * Precondition: initialCapacity > 0.
     */
    public BodyForceMap(int initialCapacity) {
        this.capacity = initialCapacity;
        this.keys = new Body[this.capacity];
        this.values = new Vector3[this.capacity];
    }

    /**
     * Adds a new key-value association to this map. If the key already exists in this map,
     * the value is replaced and the old value is returned. Otherwise 'null' is returned.
     * Precondition: key != null.
     */
    public Vector3 put(Body key, Vector3 force) {
        if (size == capacity) {
            doubleCapacity();
        }

        int left = 0;
        int right = size - 1;
        while (left <= right) {
            int middle = left + (right - left) / 2;
            if (keys[middle] == key) {
                Vector3 v = values[middle];
                values[middle] = force;
                return v;
            } else if (keys[middle].mass() < key.mass()) {
                right = middle - 1;
            } else {
                left = middle + 1;
            }
        }

        int insert = right + 1;

        for (int i = size; i > insert; i--) {
            keys[i] = keys[i - 1];
            values[i] = values[i - 1];
        }
        size++;
        keys[insert] = key;
        values[insert] = force;
        return null;
    }

    /**
     * Returns the value associated with the specified key, i.e. the returns the force vector
     * associated with the specified body. Returns 'null' if the key is not contained in this map.
     * Precondition: key != null.
     */
    public Vector3 get(Body key) {
        int left = 0;
        int right = size - 1;

        while (left <= right) {
            int middle = left + ((right - left) / 2);
            int middleLeft = left + (middle - left) / 2;
            int middleRight = middle + (right - middle) / 2;

            if (keys[middle] == key) {
                return values[middle];
            }

            if (keys[middleLeft].mass() == keys[middle].mass()) {
                for (int i = middleLeft; i < middle; i++) {
                    if (keys[i] == key)
                        return values[i];
                }
            }
            if (keys[middle].mass() == keys[middleRight].mass()) {
                for (int i = middle; i < middleRight; i++) {
                    if (keys[i] == key)
                        return values[i];
                }
            }

            if (keys[middle].mass() < key.mass()) {
                right = middle - 1;
            } else {
                left = middle + 1;
            }
        }

        return null;
    }

    /**
     * Doubles the capacity of the map.
     */
    private void doubleCapacity() {
        capacity *= 2;
        Body[] tmpKeys = new Body[capacity];
        Vector3[] tmpValues = new Vector3[capacity];

        for (int i = 0; i < size; i++) {
            tmpKeys[i] = keys[i];
            tmpValues[i] = values[i];
        }

        keys = tmpKeys;
        values = tmpValues;
    }
}
