PackedLongQueue.scala

package net.tixxit.snippet

import scala.collection.IterableLike
import scala.collection.generic.CanBuildFrom
import scala.collection.mutable.Builder

import scala.annotation.tailrec

import spire.syntax._

final class FixedQueue(val capacity: Int) {
  var buffer: Array[Long] = new Array[Long](capacity)
  private var head: Int = 0
  private var tail: Int = 0
  private var size0: Int = 0

  def clear() { head = 0; tail = 0; size0 = 0 }
  def size: Int = size0
  def isEmpty: Boolean = size0 == 0
  def isFull: Boolean = size0 == capacity

  def copy: FixedQueue = {
    val q = new FixedQueue(capacity)
    System.arraycopy(buffer, 0, q.buffer, 0, capacity)
    q.head = head
    q.tail = tail
    q.size0 = size0
    q
  }

  def flush(): Array[Long] = {
    val buf0 = new Array[Long](size)
    if (head < tail) {
      System.arraycopy(buffer, head, buf0, 0, buf0.length)
    } else {
      val prefix = buffer.length - head
      System.arraycopy(buffer, head, buf0, 0, prefix)
      System.arraycopy(buffer, 0, buf0, prefix, tail)
    }
    clear()
    buf0
  }

  def dequeue(): Long = {
    require(!isEmpty, "dequeue from empty queue!")
    val n = buffer(head)
    head = (head + 1) % buffer.length
    size0 -= 1
    n
  }

  def enqueue(n: Long) {
    require(!isFull, "enqueue on full queue!")
    buffer(tail) = n
    tail = (tail + 1) % buffer.length
    size0 += 1
  }
}

object PackedLongQueue {
  def apply(): PackedLongQueue = {
    val queue = new PackedLongQueue()
    queue.initialize()
    queue
  }

  def empty = apply()

  private val BufferSize = 1024
  private final val InitialBytes = 1024

  private final val PackedSigned = 0x0: Byte
  private final val PackedUnsigned = 0x1: Byte

  implicit def canBuildFrom: CanBuildFrom[PackedLongQueue, Long, PackedLongQueue] =
    new CanBuildFrom[PackedLongQueue, Long, PackedLongQueue] {
      def apply(from: PackedLongQueue) = {
        val q = PackedLongQueue.empty
        q ++= from
        q
      }
      def apply() = PackedLongQueue.empty
    }
}

final class PackedLongQueue private () extends Iterable[Long]
with IterableLike[Long, PackedLongQueue]
with Builder[Long, PackedLongQueue] {
  import PackedLongQueue._

  private var prefix: FixedQueue = null
  private var suffix: FixedQueue = null 
  private var bytes: Array[Byte] = null
  private var head = 0
  private var tail = 0
  private var size0 = 0

  private def initialize() {
    prefix = new FixedQueue(BufferSize)
    suffix = new FixedQueue(BufferSize)
    bytes = new Array[Byte](InitialBytes)
  }

  private def unsafeCopy: PackedLongQueue = {
    val q = new PackedLongQueue()
    q.prefix = prefix.copy
    q.suffix = suffix.copy
    q.bytes = bytes
    q.head = head
    q.tail = tail
    q.size0 = size0
    q
  }

  def copy: PackedLongQueue = {
    val q = new PackedLongQueue()
    q.prefix = prefix.copy
    q.suffix = suffix.copy
    q.bytes = java.util.Arrays.copyOf(bytes, bytes.length)
    q.head = head
    q.tail = tail
    q.size0 = size0
    q
  }

  def byteSize: Int = (tail - head) + 16 * BufferSize
  override def isEmpty: Boolean = head == tail && prefix.isEmpty && suffix.isEmpty
  override def size: Int = BufferSize * size0 + prefix.size + suffix.size
  override def newBuilder = PackedLongQueue.empty

  def +=(n: Long) = { enqueue(n); this }

  def clear() {
    prefix.clear()
    suffix.clear()
    bytes = new Array[Byte](InitialBytes)
    head = 0
    tail = 0
    size0 = 0
  }

  def result() = this

  def iterator: Iterator[Long] = new Iterator[Long] {
    private val queue = unsafeCopy
    def hasNext = !queue.isEmpty
    def next() = queue.dequeue()
  }

  def enqueue(n: Long) {
    if (suffix.isFull) {
      packAll(suffix.flush())
      size0 += 1
    }
    suffix.enqueue(n)
  }

  def dequeue(): Long = {
    if (prefix.isEmpty) {
      if (size0 == 0) {
        suffix.dequeue()
      } else {
        unpackAll(prefix)
        size0 -= 1
        prefix.dequeue()
      }
    } else {
      prefix.dequeue()
    }
  }

  private def unpackAll(buffer: FixedQueue) {
    prefix.clear()
    unpack().toByte match {
      case PackedUnsigned =>
        cfor(0)(_ < BufferSize, _ + 1) { _ =>
          buffer.enqueue(unpack())
        }

      case PackedSigned =>
        cfor(0)(_ < BufferSize, _ + 1) { _ =>
          buffer.enqueue(unpackSigned())
        }
    }
  }

  private def packAll(buffer: Array[Long]) {
    if (isUnsigned(buffer)) {
      pack(PackedUnsigned)
      cfor(0)(_ < buffer.length, _ + 1) { i =>
        pack(buffer(i))
      }
    } else {
      pack(PackedSigned)
      cfor(0)(_ < buffer.length, _ + 1) { i =>
        packSigned(buffer(i))
      }
    }
  }

  private def isUnsigned(buffer: Array[Long]): Boolean = {
    var i = 0
    while (i < buffer.length && buffer(i) >= 0) {
      i += 1
    }
    i == buffer.length
  }

  @tailrec
  private final def reqBytes(nbytes: Int) {
    if ((tail + nbytes) > bytes.length) {
      if (head > tail / 2) {
        System.arraycopy(bytes, head, bytes, 0, tail - head)
        tail = tail - head
        head = 0
        reqBytes(nbytes)
      } else {
        val bytes0 = new Array[Byte](2 * bytes.length)
        System.arraycopy(bytes, 0, bytes0, 0, bytes.length)
        bytes = bytes0
        reqBytes(nbytes - bytes.length)
      }
    }
  }

  @inline private final def put(b: Byte) {
    bytes(tail) = b
    tail += 1
  }

  @inline private final def get(): Byte = {
    val b = bytes(head)
    head += 1
    b
  }

  private final def packSigned(n0: Long) {
    reqBytes(10)
    val flag = if (n0 < 0) 0x40 else 0
    val n = math.abs(n0)
    val h = n & 0x3FL
    val t = n >>> 6
    if (t > 0) {
      put((h | 0x80 | flag).toByte)
      pack0(t)
    } else {
      put((h | flag).toByte)
    }
  }

  final def pack(n: Long) {
    reqBytes(10)
    pack0(n)
  }

  @tailrec
  private final def pack0(n: Long) {
    val h = n & 0x7FL
    val t = n >>> 7
    if (t > 0) {
      put((h | 0x80).toByte)
      pack0(t)
    } else {
      put(h.toByte)
    }
  }

  private final def unpackSigned(): Long = {
    val b = get()
    val h = b.toLong & 0x3FL
    val n = if ((b & 0x80) != 0) unpack0(6, h) else h
    if ((b & 0x40) != 0) -n else n
  }

  private final def unpack(): Long = unpack0(0, 0L)

  @tailrec
  private final def unpack0(width: Int, prev: Long): Long = {
    val b = get()
    val h = b.toLong & 0x7FL
    val cur = prev | (h << width)
    if ((b & 0x80) != 0) unpack0(width + 7, cur) else cur
  }
}