/*
 * queue.c
 *
 * Created: 7/8/2019 11:59:16 AM
 */

#include "queue.h"

//---------------------------------------------------
// setup_queue
// in:
//   q = address of queue structure
//   buffer = address of buffer
//   buffer_size = size of buffer
// returns:
//   None, queue is initialized.
void setup_queue( struct q_struct *q, char *buffer, unsigned buffer_size )
{
   q->q_start = q->q_in = q->q_out = buffer;
   q->q_end = buffer + buffer_size;
}

//---------------------------------------------------
// char_into_queue
// in:
//   q = address of queue structure
//   c = char to add to queue
// returns:
//   -1 if q isn't valid
//    0 if q is full
//    1 for success
//   Changes q_out pointer.
int char_into_queue( Q_STRUCT *q, char c )
{
   char *next;

   // New character is stored at q_in, unless buffer is full.
   // Buffer is full if new q_in pointer would equal q_out.
   if( q->q_in != 0 ) {
      *q->q_in = c;   //store the character
                      //
                      // update the pointer
                      //
      next = q->q_in + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      //
      // Compare the updated pointer to the q_out pointer.
      // If equal, the queue is full, do not store the updated pointer.
      //
      if( next == q->q_out ) {
         return 0;
      }
      else {
         //
         // update the q_in pointer.
         //
         q->q_in = next;
         return 1;
      }
   }
   return (-1);
}

//---------------------------------------------------
// char_outof_queue
// in:
//   q = address of queue structure
// returns:
//   character ( -1 if no character in queue)
//   Changes q_out pointer.
int char_outof_queue( Q_STRUCT *q )
{
   char *next;
   int ch;

   // New character is read from buffer at q_out, unless buffer is empty.
   // Buffer is empty if q_in pointer equals q_out.
   //
   if( q->q_out != 0 && q->q_out != q->q_in ) {
      ch = (int)*q->q_out;
      next = q->q_out + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      q->q_out = next;
      return ch;
   }
   else {
      return (-1);
   }
}

//---------------------------------------------------
// num_chars_queued
// in:
//   q = address of queue structure
// returns:
//   number of bytes that are in the queues
unsigned short num_chars_queued( Q_STRUCT *q ){
   
	unsigned short  num;  //limit buffer size to < 65535
	unsigned long  get;
	unsigned long  put;
	unsigned long  size;
 
	//get = pPrivFifo->get;
	//put = pPrivFifo->put;
	//size = pPrivFifo->size;

   get = q->q_out - q->q_start;
   put = q->q_in - q->q_start;
   size = q->q_end - q->q_start;
   
	if (put >= get) {
		num = put - get;
	} else {
		num = size + put - get;
	}

   // return the number of bytes in the queue
   return num;
   
}

//---------------------------------------------------
// word_setup__queue
// in:
//   q = address of queue structure buffer = address of buffer buffer_size = size of
//   buffer
// returns:
//   None, queue is initialized.
void word_setup_queue( struct word_q_struct *q, unsigned short *buffer, unsigned buffer_size )
{
   q->q_start = q->q_in = q->q_out = buffer;
   q->q_end = buffer + buffer_size;
}

//---------------------------------------------------
// word_into_queue
// in:
//   q = address of queue structure c = char to add to queue
// returns:
//   -1 if q isn't valid
//    0 if q is full
//    1 for success
//   Changes q_out pointer.
int word_into_queue( WORD_Q_STRUCT *q, unsigned short c )
{
   unsigned short *next;

   // New character is stored at q_in, unless buffer is full.
   // Buffer is full if new q_in pointer would equal q_out.
   if( q->q_in != 0 ) {
      *q->q_in = c;   //store the character
                      //
                      // update the pointer
                      //
      next = q->q_in + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      //
      // Compare the updated pointer to the q_out pointer.
      // If equal, the queue is full, do not store the updated pointer.
      //
      if( next == q->q_out ) {
         return 0;
      }
      else {
         //
         // update the q_in pointer.
         //
         q->q_in = next;
         return 1;
      }
   }
   return (-1);
}

//---------------------------------------------------
// word_outof_queue
// in:
//   q = address of queue structure
// returns:
//   character ( -1 if no character in queue)
//   Changes q_out pointer.
int word_outof_queue( WORD_Q_STRUCT *q )
{
   unsigned short *next;
   int ch;

   // New character is read from buffer at q_out, unless buffer is empty.
   // Buffer is empty if q_in pointer equals q_out.
   //
   if( q->q_out != 0 && q->q_out != q->q_in ) {
      ch = (int)*q->q_out;
      next = q->q_out + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      q->q_out = next;
      return ch;
   }
   else {
      return (-1);
   }
}

//---------------------------------------------------
// setup_sword_queue
// in:
//   q = address of queue structure
//   buffer = address of buffer
//   buffer_size = size of buffer
// returns:
//   None, queue is initialized.
void sword_setup_queue( struct sword_q_struct *q, signed short *buffer, unsigned buffer_size )
{
   q->q_start = q->q_in = q->q_out = buffer;
   q->q_end = buffer + buffer_size;
}

//---------------------------------------------------
// char_into_queue
// in:
//   q = address of queue structure
//   c = char to add to queue
// returns:
//   -1 if q isn't valid
//    0 if q is full
//    1 for success
//   Changes q_out pointer.
int sword_into_queue( SWORD_Q_STRUCT *q, signed short c )
{
   signed short *next;

   // New character is stored at q_in, unless buffer is full.
   // Buffer is full if new q_in pointer would equal q_out.
   if( q->q_in != 0 ) {
      *q->q_in = c;   //store the character
                      //
                      // update the pointer
                      //
      next = q->q_in + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      //
      // Compare the updated pointer to the q_out pointer.
      // If equal, the queue is full, do not store the updated pointer.
      //
      if( next == q->q_out ) {
         return 0;
      }
      else {
         //
         // update the q_in pointer.
         //
         q->q_in = next;
         return 1;
      }
   }
   return (-1);
}

//---------------------------------------------------
// char_outof_queue
// in:
//   q = address of queue structure
// returns:
//   character ( -1 if no character in queue)
//   Changes q_out pointer.
int sword_outof_queue( SWORD_Q_STRUCT *q )
{
   signed short *next;
   int ch;

   // New character is read from buffer at q_out, unless buffer is empty.
   // Buffer is empty if q_in pointer equals q_out.
   //
   if( q->q_out != 0 && q->q_out != q->q_in ) {
      ch = (int)*q->q_out;
      next = q->q_out + 1;
      //
      // Compare the updated pointer to the end of the buffer.
      // If at the end, wrap the pointer to the start.
      //
      if( next == q->q_end ) {
         next = q->q_start;
      }
      q->q_out = next;
      return ch;
   }
   else {
      return (-1);
   }
}
