#include <stdio.h>
#include <exec/types.h>
#include "showprioq.h"

char *malloc();

struct prioq_struct *pq_new (index_size, value_size)
  int index_size, value_size;
  {
  struct prioq_struct *pq;
  unsigned char *tmp_array;
  int i;

  if (index_size > 256)
    return (NULL);

  if ((pq = (struct prioq_struct *) malloc
        (sizeof (struct prioq_struct))) == NULL)
    return (NULL);

  if ((pq -> queue = (struct q_entry *)
                  malloc (index_size * sizeof (struct q_entry))) == NULL)  {
    free (pq);
    return (NULL);
    }
    
  if ((pq -> array = (unsigned char *) malloc (value_size)) == NULL) {
    free (pq -> queue);
    free (pq);
    return (NULL);
    }

  for (i=0, tmp_array = pq -> array ; i<value_size ; i++, tmp_array++)
    *tmp_array = 0;

  pq -> queue_size = index_size;
  pq -> array_size = value_size;
  pq -> current_entry = 0;
  return (pq);
  }

pq_add (q, index, value)
  struct prioq_struct *q;
  unsigned int index, value;
  {
  unsigned int existing_entry;

  if (value >= q -> array_size)
    return (0);

  if ((existing_entry = pq_find_value(q, value)) != 0) {
    if ((q -> queue[existing_entry].index) < index)
      (q -> queue[existing_entry].index) = index;
    pq_balance (q, existing_entry);
    }
  else {
    q -> current_entry++;
    if (q -> current_entry >= q -> queue_size) {
      q -> current_entry--;
      q -> array [q->queue[q->current_entry].value] = 0;
      }

    q -> queue [q -> current_entry].index = index;
    q -> queue [q -> current_entry].value = value;
    q -> array [value] = q -> current_entry;
    pq_balance (q, q -> current_entry);
    }
  }
    
pq_find_value (q, value)
  struct prioq_struct *q;
  unsigned int value;
  {
  if (value < q -> array_size)
    return (q -> array[value]);
  else
    return (0);
  }

pq_balance(q, entry_pos1)
  struct prioq_struct *q;
  unsigned int entry_pos1;
  {
  register struct q_entry *entry1, *entry2;
  register unsigned int tmp_index, tmp_value, entry_pos2;

  entry1 = &q->queue[entry_pos1];

  if ((entry_pos1 * 2 < q->queue_size)
      && (entry_pos1 * 2 <= q -> current_entry))
    {
    if ((entry_pos1*2+1 > q->current_entry) ||
        (q->queue[entry_pos1*2].index > q->queue[entry_pos1*2+1].index))
      entry_pos2 = entry_pos1*2;
    else
      entry_pos2 = entry_pos1*2+1;

    entry2 = &q->queue[entry_pos2];

    if (entry1->index < entry2->index) {
      q -> array [entry1->value] = entry_pos2;
      q -> array [entry2->value] = entry_pos1;

      tmp_index = entry1->index;
      entry1->index = entry2->index;
      entry2->index = tmp_index;

      tmp_value = entry1->value;
      entry1->value = entry2->value;
      entry2->value = tmp_value;

      pq_balance (q, entry_pos2);
      }
    }

  if (entry_pos1 / 2 >= 1 )
    {
    entry_pos2 = entry_pos1 / 2;
    entry2 = &q->queue[entry_pos2];
    if (entry1->index > entry2->index) {
      q -> array [entry1->value] = entry_pos2;
      q -> array [entry2->value] = entry_pos1;

      tmp_index = entry1->index;
      entry1->index = entry2->index;
      entry2->index = tmp_index;

      tmp_value = entry1->value;
      entry1->value = entry2->value;
      entry2->value = tmp_value;

      pq_balance (q, entry_pos2);
      }
    }
  }

pq_get_highest_index(q)
  struct prioq_struct *q;
  {
  if (q -> current_entry >= 1)
    return (q -> queue[1].index);
  else
    return (0);
  }

pq_get_highest_value(q)
  struct prioq_struct *q;
  {
  if (q -> current_entry >= 1)
    return (q -> queue[1].value);
  else
    return (0);
  }

pq_delete_highest (q)
  struct prioq_struct *q;
  {
  q -> queue[1].index = q -> queue[q -> current_entry].index;
  q -> queue[1].value = q -> queue[q -> current_entry--].value;
  pq_balance (q, 1);
  }

pq_free (q)
  struct prioq_struct *q;
  {
  free (q ->queue);
  free (q -> array);
  free (q);
  }