DuoHeap

package test;

import java.util.Comparator;
import java.util.Random;
import java.util.TreeSet;

public class DoubleQueueTest {
    private void main() {
        // long seed = 0xd6592198ce49bf7dL;
        long seed = new Random().nextLong();
        System.out.println("Seed: " + Long.toUnsignedString(seed, 16));
        ran = new Random(seed);

        Comparator<Complex> c1 = Comparator.comparingInt(Complex::getReal)
                .thenComparingInt(Complex::getId);
        Comparator<Complex> c2 = Comparator.comparingInt(Complex::getImaginary)
                .thenComparingInt(Complex::getId);

        DuoHeapDoubleQueue<Complex> heap = new DuoHeapDoubleQueue<>();

        heap.init(c1, c2);

        // correctness test
        for (int i = 0; i < 13000; i++) {
            Complex c = rand();
            System.out.println("Epoch: " + i + ", generated: " + c);
            heap.add(c);
            if (i > 0 && i % 5 == 0) {
                heap.poll1();
            }
            if (i > 0 && i % 7 == 0) {
                heap.poll2();
            }
            System.out.println("Peek1: " + heap.peek1());
            System.out.println("Peek2: " + heap.peek2());
            heap.inspect();
        }

        System.out.println("Inspection fails: " + inspectionFails);
        System.out.println("Inspection checks: " + inspectionChecks);

        // performace test
        ran.setSeed(seed);
        DuoHeapDoubleQueue<Complex> heap1 = new DuoHeapDoubleQueue<>();
        heap1.init(c1, c2);
        long start1 = System.nanoTime();
        for (int i = 0; i < 13000; i++) {
            Complex c = rand();
            heap1.add(c);
            if (i > 0 && i % 5 == 0) {
                heap1.poll1();
            }
            if (i > 0 && i % 7 == 0) {
                heap1.poll2();
            }
        }
        long end1 = System.nanoTime();
        System.out.println("Duo heap: " + (end1 - start1));

        // performace test, part 2
        ran.setSeed(seed);
        Priority2Queue<Complex> heap2 = new Priority2Queue<>(c1, c2);
        long start2 = System.nanoTime();
        for (int i = 0; i < 13000; i++) {
            Complex c = rand();
            heap2.add(c);
            if (i > 0 && i % 5 == 0) {
                heap2.poll1();
            }
            if (i > 0 && i % 7 == 0) {
                heap2.poll2();
            }
        }
        long end2 = System.nanoTime();
        System.out.println("Duo tree: " + (end2 - start2));
    }

    class Foo {
        Foo() {
            super();
        }
    }

    private Random ran;
    private int min = -100, max = 100;

    private Complex rand() {
        int r = ran.nextInt(max - min + 1) + min;
        int i = ran.nextInt(max - min + 1) + min;
        return new Complex(r, i);
    }

    private static class Complex {
        private int real;
        private int imaginary;
        private int id;
        private static int idCount = 0;

        public int getReal() {
            return real;
        }

        public int getImaginary() {
            return imaginary;
        }

        public int getId() {
            return id;
        }

        @Override
        public int hashCode() {
            final int prime = 31;
            int result = 1;
            result = prime * result + imaginary;
            result = prime * result + real;
            result = prime * result + id;
            return result;
        }

        @Override
        public String toString() {
            return "Complex [real=" + real + ", imaginary=" + imaginary
                    + ", id=" + id + "]";
        }

        @Override
        public boolean equals(Object obj) {
            if (this == obj)
                return true;
            if (obj == null)
                return false;
            if (getClass() != obj.getClass())
                return false;
            Complex other = (Complex) obj;
            if (imaginary != other.imaginary)
                return false;
            if (real != other.real)
                return false;
            if (id != other.id)
                return false;
            return true;
        }

        public Complex(int real, int imaginary) {
            this.real = real;
            this.imaginary = imaginary;
            id = idCount++;
        }
    }

    public static void main(String[] args) {
        new DoubleQueueTest().main();
    }

    private interface DoubleQueue<T> {

        public void init(Comparator<? super T> c1, Comparator<? super T> c2);

        public void add(T e);

        public T peek1();

        public T peek2();

        public T poll1();

        public T poll2();
    }

    private int inspectionFails = 0;
    private long inspectionChecks = 0;

    private class DuoHeapDoubleQueue<T> implements DoubleQueue<T> {
        private Comparator<? super T>[] cs;

        private T[][] data;
        private int[][] links;

        private int count; // 1 = no items
        private static final int CAPACITY = 10000;

        @SuppressWarnings({ "unchecked" })
        @Override
        public void init(Comparator<? super T> c1, Comparator<? super T> c2) {

            cs = new Comparator[2];
            cs[0] = c1;
            cs[1] = c2;

            data = (T[][]) new Object[2][CAPACITY + 1];

            links = new int[2][CAPACITY + 1];

            count = 1;
        }

        /**
         * swaps elements in one heap and sets links accordingly
         *
         * @param heapID
         *            0 or 1
         * @param pos0
         *            indexed from 1
         * @param pos1
         *            indexed from 1
         */
        private void swap(int heapID, int pos0, int pos1) {
            // acctual swap
            T[] heap = data[heapID];
            T tmp = heap[pos0];
            heap[pos0] = heap[pos1];
            heap[pos1] = tmp;

            // index update 1, swapping via XOR
            int[] links = this.links[heapID];
            links[pos0] ^= links[pos1];
            links[pos1] ^= links[pos0];
            links[pos0] ^= links[pos1];

            // index update 2
            int[] links2 = this.links[1 - heapID];
            links2[links[pos0]] = pos0;
            links2[links[pos1]] = pos1;
        }

        private int parent(int position) {
            return position >>> 1;
        }

        private int right(int position) {
            return (position << 1) + 1;
        }

        private int left(int position) {
            return position << 1;
        }

        private boolean exists(int position) {
            return position > 0 && position < count;
        }

        @Override
        public void add(T e) {
            if (count > CAPACITY) {
                System.err.println("FULL");
                return;
            }
            data[0][count] = data[1][count] = e;
            links[0][count] = links[1][count] = count;

            count++;

            bubbleUpAt(0, count - 1);
            bubbleUpAt(1, count - 1);
        }

        private void bubbleUpAt(int heapID, int pos) {
            if (pos == 1) { // root
                return;
            }
            T[] heap = data[heapID];
            int parent = parent(pos);
            if (cs[heapID].compare(heap[pos], heap[parent]) >= 0) {
                // parent is lower or equal
                return;
            }
            swap(heapID, pos, parent);
            bubbleUpAt(heapID, parent);
        }

        public void inspect() {
            System.out.println("  --  INSPECTION START  --  ");
            inspectHeap(0);
            inspectHeap(1);
            inspectLinks(0);
            inspectLinks(1);
            System.out.println("  --   INSPECTION END   --  ");
        }

        private void inspectHeap(int heapID) {
            System.out.println("  - inspecting heap: " + heapID);
            for (int i = 1; i <= count; i++) {
                inspectNode(heapID, i);
            }
        }

        private void inspectNode(int heapID, int pos) {
            int left = left(pos);
            int right = right(pos);

            T[] heap = data[heapID];
            if (exists(left)) {
                if (cs[heapID].compare(heap[pos], heap[left]) > 0) {
                    System.out.format(
                            "   ! inspection FAIL in heap %d at pos %d%n",
                            heapID, pos);
                    err();
                }
            }
            if (exists(right)) {
                if (cs[heapID].compare(heap[pos], heap[right]) > 0) {
                    System.out.format(
                            "   ! inspection FAIL in heap %d at pos %d%n",
                            heapID, pos);
                    err();
                }
            }
            inspectionChecks++;
        }

        private void inspectLinks(int id) {
            System.out.println("  - inspecting links: " + id);
            T[] heap = data[id];
            T[] heap2 = data[1 - id];
            int[] links = this.links[id];
            for (int i = 1; i < count; i++) {
                if (heap[i] != heap2[links[i]]) {
                    System.out.format(
                            "   ! inspection FAIL in links %d at pos %d%n", id,
                            i);
                    err();
                }
            }
            inspectionChecks++;
        }

        private void err() {
            inspectionFails++;
        }

        @Override
        public T peek1() {
            return count == 1 ? null : data[0][1];
        }

        @Override
        public T peek2() {
            return count == 1 ? null : data[1][1];
        }

        private T poll(int heapID) {
            if (count == 1) { // empty
                return null;
            }
            T ret = data[heapID][1];
            int pos2 = links[heapID][1];

            count--;
            remove(heapID, 1);
            remove(1 - heapID, pos2);

            return ret;
        }

        /**
         * removes selected element, count has already been reduced
         *
         * @param heapID
         *            id
         * @param pos
         *            indexed from 1
         */
        private void remove(int heapID, int pos) {
            if (!exists(pos)) {
                // removed elemenent was last ^_^
                return;
            }
            swap(heapID, pos, count);

            T[] heap = data[heapID];

            // see if needs bubbling up
            int parent = parent(pos);
            if (exists(parent)
                    && cs[heapID].compare(heap[pos], heap[parent]) < 0) {
                bubbleUpAt(heapID, pos);
            } else {
                bubbleDownAt(heapID, pos);
            }
        }

        private void bubbleDownAt(int heapID, int pos) {
            T[] heap = data[heapID];
            int min = pos; // position of minimum
            int left = left(pos);
            int right = right(pos);
            if (exists(left) && cs[heapID].compare(heap[left], heap[min]) < 0) {
                min = left;
            }
            if (exists(right) && cs[heapID].compare(heap[right], heap[min]) < 0) {
                min = right;
            }
            if (min != pos) {
                swap(heapID, min, pos);
                bubbleDownAt(heapID, min);
            }
        }

        @Override
        public T poll1() {
            return poll(0);
        }

        @Override
        public T poll2() {
            return poll(1);
        }

    }

    class Priority2Queue<E> {
        private TreeSet<E> q1;
        private TreeSet<E> q2;

        public Priority2Queue(Comparator<? super E> c1, Comparator<? super E> c2) {
            q1 = new TreeSet<E>(c1);
            q2 = new TreeSet<E>(c2);
        }

        public void add(E e) {
            q1.add(e);
            q2.add(e);
        }

        private E pollx(TreeSet<E> a, TreeSet<E> b) {
            E top = a.first();
            a.remove(top);
            b.remove(top);
            return top;
        }

        public E poll1() {
            return pollx(q1, q2);
        }

        public E poll2() {
            return pollx(q2, q1);
        }
    }

}