PriorityStack

import scala.collection.mutable

trait PriorityStack[Elem] {

  def pop(): Option[Elem]

  def push(elem: Elem): Unit

}
object PriorityStack {

  private class Impl[Elem: Ordering](
                                      initialCapacity: Int,
                                      capacityFactor: Float,
                                    ) extends PriorityStack[Elem] {

    private val ordering: Ordering[AnyRef] = new Ordering[AnyRef] {
      override def compare(x: AnyRef, y: AnyRef): Int =
        (x, y) match {
          case (null, _) => 1
          case (_, null) => -1
          case (null, null) => 0
          case _ => Ordering[Elem].compare(
            x.asInstanceOf[Elem],
            y.asInstanceOf[Elem],
          )
        }
    }

    private var underlying = new Array[AnyRef](initialCapacity)

    private var last = -1

    override def pop(): Option[Elem] =
      if(last > 0) {

        def shrink(): Unit = {
          val needShrink = (last * capacityFactor).toInt < underlying.length
          if (needShrink) {
            val shrinked = new Array[AnyRef](last)
            Array.copy(
              src = underlying,
              srcPos = 0,
              dest = shrinked,
              destPos = 0,
              length = last,
            )
          }
        }

        def head(): Option[Elem] = {
          val take = last
          last -= 1
          val elem = underlying(take).asInstanceOf[Elem]
          underlying(take) = null.asInstanceOf[AnyRef]
          Some(elem)
        }

        shrink()
        head()
      } else {
        None
      }

    override def push(elem: Elem): Unit = {

      def grow(): Unit = {
        val current = last + 1
        if(current > underlying.length) {
          val size = (current * capacityFactor).toInt
          val growed = new Array[AnyRef](size)
          Array.copy(
            src = underlying,
            srcPos = 0,
            dest = growed,
            destPos = 0,
            length = last,
          )
          underlying = growed
        }
        last = current
      }

      def insert(): Unit = {
        underlying(last) = elem.asInstanceOf[AnyRef]
        underlying.sortInPlace()(ordering)
      }

      grow()
      insert()
    }

    override def toString(): String =
      underlying.mkString("PriorityStack(", ", ", ")")

  }

  def apply[Elem: Ordering](initialCapacity: Int = 16,
                            growFactor: Float = 2f): PriorityStack[Elem] =
    new Impl[Elem](initialCapacity, growFactor)

}