/* -----------------------------------------------------------
 * Priority queue
 * -----------------------------------------------------------
 */

#include <linux/slab.h>
#include <asm/string.h>

#include "linklist.h"
#include "graph.h"
#include "pqueue.h"


/* The priority queue links existing nodes from the graph structure
   according to the distance field. Note that the priority queue
   itself does not allocate nodes (it just links them), nodes are
   allocated by the graph. */
 
struct pqueue *pqueue_new(void)
{
  struct pqueue *new;
  
  new = kmalloc(sizeof(struct pqueue), GFP_ATOMIC);
  memset (new, 0, sizeof(struct pqueue));

  pqueue_init(new);
  return new;
}

/* Initialize priority queue. Each time SPF algorithm is executed,
   priority queue must be initialized. */
void pqueue_init(struct pqueue *pq)
{
    if(pq) {
	/* Initialize priority queue */
	pq->head = NULL;
	pq->tail = NULL;
	pq->count = 0;
    }
}


void pqueue_free(struct pqueue *pq)
{
  kfree(pq);
}


void pqueue_print(struct pqueue *pq)
{
  struct node *n;

  printk(KERN_INFO "priority queue: count: %d\n", pq->count);
  
  for (n = pq->head; n; n = n->next) {
    node_print(n);
    
    printk(KERN_INFO "node: %p next: %p prev: %p distance: %d pred: %p\n",
	   n, n->next, n->prev, n->distance, n->pred);
  }
}

int pqueue_isempty(struct pqueue *pq)
{
  return (pq->count == 0);
}

void pqueue_insert(struct pqueue *pq, struct node *addnode)
{
  struct node *n;

  /* Add node to sorted doubly linked list */
  for (n = pq->head; n; n = n->next) {
    if (addnode->distance < n->distance) {       
      addnode->next = n;
      addnode->prev = n->prev;
      
      if (n->prev)
	n->prev->next = addnode;
      else
	pq->head = addnode;
      n->prev = addnode;
      pq->count++;
      return;
    } 
  }
  addnode->prev = pq->tail;

  if (pq->tail)
    pq->tail->next = addnode;
  else
    pq->head = addnode;

  pq->tail = addnode;
  pq->count++;
}

struct node *pqueue_remove_first(struct pqueue *pq)
{
  struct node *n;
  
  /* First check if priority queue is empty */
  n = pq->head;

  if (n) {
    /* Remove first element */
    if (n->prev)
      n->prev->next = n->next;
    else
      pq->head = n->next;
    
    if (n->next)
      n->next->prev = n->prev;
    else
      pq->tail = n->prev;
    
    pq->count--;
    
    /* Return first element */
    return n;
  }

  /* Priority queue empty */
  return NULL;
}

void pqueue_remove(struct pqueue *pq, struct node *n)
{
  if (n->prev)
    n->prev->next = n->next;
  else
    pq->head = n->next;
  if (n->next)
    n->next->prev = n->prev;
  else
    pq->tail = n->prev;
  pq->count--;
}
