public void add(final int key, final V value) {
        size++;

        if (first.lp == 0 && second.lp == 0) { // both full ?
            sortUp();
        }
        if (first.lp > 0) {
            first.add4(key, value);
            if (firstNonEmpty != first) {
                first.nextNonEmpty = firstNonEmpty;
                firstNonEmpty = first;
            }
        } else { // second bin not full
            second.add4(key, value);
            if (first.nextNonEmpty != second) {
                second.nextNonEmpty = first.nextNonEmpty;
                first.nextNonEmpty = second;
            }
        }

    }
add a key value pair to the heap

@param key   the key to insert
@param value the value to insert object
main/src/main/java/cgeo/geocaching/brouter/util/SortedHeap.java