/*
 *  VAS_EBOOT  --  GRand Unified Bootloader
 *  Copyright (C) 2011 Free Software Foundation, Inc.
 *
 *  VAS_EBOOT is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  VAS_EBOOT is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with VAS_EBOOT.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <VasEBoot/priority_queue.h>
#include <VasEBoot/mm.h>
#include <VasEBoot/dl.h>

VAS_EBOOT_MOD_LICENSE ("GPLv3+");

struct VasEBoot_priority_queue
{
  VasEBoot_size_t elsize;
  VasEBoot_size_t allocated;
  VasEBoot_size_t used;
  VasEBoot_comparator_t cmp;
  void *els;
};

static inline void *
element (struct VasEBoot_priority_queue *pq, VasEBoot_size_t k)
{
  return ((VasEBoot_uint8_t *) pq->els) + k * pq->elsize;
}

static inline void
swap (struct VasEBoot_priority_queue *pq, VasEBoot_size_t m, VasEBoot_size_t n)
{
  VasEBoot_uint8_t *p1, *p2;
  VasEBoot_size_t l;
  p1 = (VasEBoot_uint8_t *) element (pq, m);
  p2 = (VasEBoot_uint8_t *) element (pq, n);
  for (l = pq->elsize; l; l--, p1++, p2++)
    {
      VasEBoot_uint8_t t;
      t = *p1;
      *p1 = *p2;
      *p2 = t;
    }
}

static inline VasEBoot_size_t
parent (VasEBoot_size_t v)
{
  return (v - 1) / 2;
}

static inline VasEBoot_size_t
left_child (VasEBoot_size_t v)
{
  return 2 * v + 1;
}

static inline VasEBoot_size_t
right_child (VasEBoot_size_t v)
{
  return 2 * v + 2;
}

void *
VasEBoot_priority_queue_top (VasEBoot_priority_queue_t pq)
{
  if (!pq->used)
    return 0;
  return element (pq, 0);
}

void
VasEBoot_priority_queue_destroy (VasEBoot_priority_queue_t pq)
{
  VasEBoot_free (pq->els);
  VasEBoot_free (pq);
}

VasEBoot_priority_queue_t
VasEBoot_priority_queue_new (VasEBoot_size_t elsize,
			 VasEBoot_comparator_t cmp)
{
  struct VasEBoot_priority_queue *ret;
  void *els;
  els = VasEBoot_calloc (8, elsize);
  if (!els)
    return 0;
  ret = (struct VasEBoot_priority_queue *) VasEBoot_malloc (sizeof (*ret));
  if (!ret)
    {
      VasEBoot_free (els);
      return 0;
    }
  ret->elsize = elsize;
  ret->allocated = 8;
  ret->used = 0;
  ret->cmp = cmp;
  ret->els = els;
  return ret;
}

/* Heap property: pq->cmp (element (pq, p), element (pq, parent (p))) <= 0. */
VasEBoot_err_t
VasEBoot_priority_queue_push (VasEBoot_priority_queue_t pq, const void *el)
{
  VasEBoot_size_t p;
  if (pq->used == pq->allocated)
    {
      void *els;
      els = VasEBoot_realloc (pq->els, pq->elsize * 2 * pq->allocated);
      if (!els)
	return VasEBoot_errno;
      pq->allocated *= 2;
      pq->els = els;
    }
  pq->used++;
  VasEBoot_memcpy (element (pq, pq->used - 1), el, pq->elsize);
  for (p = pq->used - 1; p; p = parent (p))
    {
      if (pq->cmp (element (pq, p), element (pq, parent (p))) <= 0)
	break;
      swap (pq, p, parent (p));
    }

  return VAS_EBOOT_ERR_NONE;
}

void
VasEBoot_priority_queue_pop (VasEBoot_priority_queue_t pq)
{
  VasEBoot_size_t p;

  swap (pq, 0, pq->used - 1);
  pq->used--;
  for (p = 0; left_child (p) < pq->used; )
    {
      VasEBoot_size_t c;
      if (pq->cmp (element (pq, left_child (p)), element (pq, p)) <= 0
	  && (right_child (p) >= pq->used
	      || pq->cmp (element (pq, right_child (p)), element (pq, p)) <= 0))
	break;
      if (right_child (p) >= pq->used
	  || pq->cmp (element (pq, left_child (p)),
		      element (pq, right_child (p))) > 0)
	c = left_child (p);
      else
	c = right_child (p);
      swap (pq, p, c);
      p = c;
    }
}