@SuppressWarnings({"unchecked"})
    @Override
    public boolean add(T e) {
        size = size + 1;

        /*
         * Resizing the array if the size is too small.
         */
        if (queue.length <= size) {
            queue = Arrays.copyOf(queue, size * 2);
        }
        int index = size - 1;

        /*
         * Goes from the last element up through the parents. Depending on the way
         * your comparator is implemented the method would either choose the closest
         * or the furthest node from the goal node.
         */
        while (index > 0) {
            int comp = comparator.compare(e, (T) queue[getParent(index)]);
            if (comp > 0) {
                // swap the parent down to the child we came from
                queue[index] = queue[getParent(index)];
                index = getParent(index);
            } else {
                break;
            }
        }
        // the highest reached parent, gets overwritten with our new value
        queue[index] = e;
        return true;
    }