/* Naive implementation using a sorted list of pairs [key, [item[1], ..., item[n]]].
The key may be any number (integer or not). Items are extracted in FIFO order. */

defstruct(pqueue(q = []))$

/* Binary search */

find_key(q, p) := block(
   [i: 1, j: length(q), k, c],
   if j = 0 then false
   elseif (c: q[i][1]) >= p then
      (if c = p then i else false)
   elseif (c: q[j][1]) <= p then
      (if c = p then j else false)
   else catch(
      while j >= i do (
         k: quotient(i + j, 2),
         if (c: q[k][1]) = p then throw(k)
         elseif c < p then i: k + 1 else j: k - 1
      ),
      false
   )
)$

pqueue_push(pq, x, p) := block(
   [q: pq@q, k],
   k: find_key(q, p),
   if integerp(k) then q[k][2]: endcons(x, q[k][2])
   else pq@q: sort(cons([p, [x]], q)),
   'done
)$

pqueue_pop(pq) := block(
   [q: pq@q, v, x],
   if emptyp(q) then 'fail else (
      p: q[1][1],
      v: q[1][2],
      x: v[1],
      if length(v) > 1 then q[1][2]: rest(v) else pq@q: rest(q),
      x
   )
)$

pqueue_print(pq) := block([t], while (t: pqueue_pop(pq)) # 'fail do disp(t))$


/* An example */

a: new(pqueue)$

pqueue_push(a, "take milk", 4)$
pqueue_push(a, "take eggs", 4)$
pqueue_push(a, "take wheat flour", 4)$
pqueue_push(a, "take salt", 4)$
pqueue_push(a, "take oil", 4)$
pqueue_push(a, "carry out crepe recipe", 5)$
pqueue_push(a, "savour !", 6)$
pqueue_push(a, "add strawberry jam", 5 + 1/2)$
pqueue_push(a, "call friends", 5 + 2/3)$
pqueue_push(a, "go to the supermarket and buy food", 3)$
pqueue_push(a, "take a shower", 2)$
pqueue_push(a, "get dressed", 2)$
pqueue_push(a, "wake up", 1)$
pqueue_push(a, "serve cider", 5 + 3/4)$
pqueue_push(a, "buy also cider", 3)$

pqueue_print(a);
"wake up"
"take a shower"
"get dressed"
"go to the supermarket and buy food"
"buy also cider"
"take milk"
"take butter"
"take flour"
"take salt"
"take oil"
"carry out recipe"
"add strawberry jam"
"call friends"
"serve cider"
"savour !"