#include "PQ.h"
#include "node.h"
#include "parameters.h"

//----------------------------------- internal
int PQ_size;
AstarNode **PQ_queue;
void PQ_internal_adjust(void);
//----------------------------------- 


// initialize the priority queue
void PQ_init(void)
{
 PQ_size = 0;
 // Allocate space for MAX_QUEUE_SIZE nodes, plus doclen more nodes 
 // to allow addition of all successors of a parent node.
 // But the queue will be trimmed back to MAX_QUEUE_SIZE later.
 if ((PQ_queue = (AstarNode **)malloc((MAX_QUEUE_SIZE+Astar_doclen)*sizeof(AstarNode *))) == NULL)
     {perror("error malloc");
      exit(EXIT_FAILURE);
     }
}


// free all remaining nodes in the priority queue
void PQ_free_all(void)
{int i;
 for (i=0; i<PQ_size; i++)
   node_delete(PQ_queue[i]);
 free(PQ_queue);
}


// boolean
int PQ_empty(void)
{return PQ_size==0;}

// boolean
int PQ_queue_overflow(void)
{return PQ_size>MAX_QUEUE_SIZE;}



// add an item to the priority queue
void PQ_push(AstarNode *node)
{
 #ifdef ASTAR_DEBUG
 if (PQ_size >= MAX_QUEUE_SIZE+Astar_doclen)
 	{perror("PQ_queue overflow!");
	 exit(EXIT_FAILURE);
	}
 #endif
 PQ_queue[PQ_size++] = node;
 PQ_internal_adjust(); // maintain a PQ

 //printf("[%d] ", PQ_size);
}


// pop out (i.e. remove) the item with the best (highest) f score
AstarNode *PQ_pop_best(void)
{AstarNode *best_node;
 if (PQ_size<=0)
 	{perror("PQ_queue underflow!");
	 exit(EXIT_FAILURE);
	}
 best_node = PQ_queue[0];
 PQ_queue[0] = PQ_queue[PQ_size-1]; // fill in the hole
 PQ_size--;
 PQ_internal_adjust(); // maintain a PQ
 return best_node;
}


// pop out the item with the worst (smallest) f score
AstarNode *PQ_pop_worst(void)
{AstarNode *worst_node;
 if (PQ_size<=0)
 	{perror("PQ_queue underflow!");
	 exit(EXIT_FAILURE);
	}
 worst_node = PQ_queue[PQ_size-1]; // in this implementation 
 PQ_size--;
 // no need to further adjust the sorted queue
 return worst_node;
}



int PQ_compare_nodes(const void *a, const void *b)
{
 AstarNode **pa = (AstarNode **) a;
 AstarNode **pb = (AstarNode **) b;
 AstarNode *nodea = *pa;
 AstarNode *nodeb = *pb;

 // major key: f=g+h (bigger is better)
 // minor key: partial_length (bigger is better)
 if (nodea->g + nodea->h > nodeb->g + nodeb->h)
	return -1;
 else if (nodea->g + nodea->h < nodeb->g + nodeb->h)
	return 1;
 else // it's a tie in f
      if (nodea->partial_length > nodeb->partial_length)
	return -1;
      else if (nodea->partial_length < nodeb->partial_length)
	return 1;
      else
	return 0;
}

void PQ_internal_adjust(void)
{
 // lazy man's implementation of double ended priority queue: a sorted list, best (largest) value first.
 qsort(PQ_queue, PQ_size, sizeof(AstarNode *), PQ_compare_nodes);
}