#include <stdlib.h>
#include "sly.h"

unsigned char data[2048];

// ======================================
// void init_data()
// Purpose: Zero-out the data array
// ======================================
void init_data ()
{
   int i;
   for (i = 0; i < 2048; i++)
      data[i] = 0;
}

// ======================================
// void onOutOfMemory(void)
// Purpose: exit on out-of-memory error
// ======================================
void onOutOfMemory(void)
{
   printf ("Out of memory\n");
   exit(1);
}

// ======================================
// void onIllegalOperation(void)
// Purpose: exit on illegal operation error
// ======================================
void onIllegalOperation(void)
{
   printf ("Illegal operation\n");
   exit(2);
}

// =====================================================
// void get_queue_data (int marker, int *id, int *size)
// Purpose: Quickly report queue data at
//          current space in array (marked by marker)
// Input: int marker (space in array to analyze, 0-2047)
// Output: int *id (queue_id, or -1 if not a queue)
//         int *size (queue_size, or -1 if no queue)
// =====================================================
void get_queue_data (int marker, int *id, int *size)
{
   int queue_size = -1;
   int queue_id = -1;
   unsigned char header;

   if ((data[marker] & 0x80) == 0x80)
      {
         queue_size = 0;
         header = data[marker];
         // Last bit in this byte worth 256 in queue size

         if ((header & 0x01) == 0x01)
            queue_size += 256;

         header >>= 1;   // shift 6 ID bits to the right
         header &= 0x3F; // keep only six right-most bits

         queue_id = header;
   
         // Add value of remaining queue_size byte
         queue_size += data[marker+1];
      }

   if (id)
      *id = queue_id;
   if (size)
      *size = queue_size;
}

// Create a queue, return queue_id handle
Q* createQ()
{
   Q q;
   int i, marker;
   // I'm splurging and using chars for these booleans to save time
   unsigned char id_used [64];
   int queue_id;
   unsigned char header;
   int queue_size;
   
   for (i = 0; i < 64; i++)
      id_used[i] = 0;

   marker = 0;

   // Find the first available queue space
   while (marker < 2048)
      {
         // Check for is_queue bit set (0x80)
         if ((data[marker] & 0x80) == 0x80)
            {
               get_queue_data (marker, &queue_id, &queue_size);
               id_used [queue_id] = 1;

               // Move marker ahead to next possible queue header
               marker += queue_size + 2;
            }
         else break;
      }
   
   if (marker >= 2048)
      {
         // out of space, failed to create queue
         onOutOfMemory();
      }

   for (i = 0; i < 64; i++)
      {
          if (id_used[i] == 0)
             {
                // Select this unused queue_id for the new queue
                // Current byte should be zero
                data[marker] = i;
                // Shift id left one byte
                data[marker] <<= 1;
                data[marker] |= 0x80;
                return (Q*) i;
             }
      }

  // failed, no more queue space (all 64 queue_ids used up)
  onOutOfMemory();
}

// Destroy specified queue
void destroyQ (Q *q)
{
   unsigned char delete_this = (unsigned char) q;
   int marker = 0, queue_id, queue_size;
   int copy_from, check_for_q;

   while (marker < 2048)
   {
      get_queue_data (marker, &queue_id, &queue_size);
      if (queue_id == (int) delete_this)
         {
            // Delete this queue; shift all following data down
            copy_from = marker + queue_size + 2;
            check_for_q = copy_from;
            while (copy_from < 2048)
               {
                  if (copy_from == check_for_q)
                     {
                        get_queue_data (copy_from, &queue_id, &queue_size);
                        if (queue_id >= 0)
                           check_for_q = copy_from + queue_size + 2;
                        else
                           check_for_q = -1;
                        if (queue_id == (int) delete_this)
                           {
                             // found another segment, delete it too
                             copy_from += queue_size + 2;
                           }
                        else
                           data[marker++] = data[copy_from++];
                     }
                  else
                     data[marker++] = data[copy_from++];
               }
            while (marker < 2048)
               data[marker++] = 0;
         }
      else
         marker += queue_size + 2;
   }
}

// Enqueue specified byte in specified queue
void enQ (Q *q, unsigned char b)
{
   unsigned char add_to_this = (unsigned char) q;
   int marker = 0;
   int queue_id, queue_size;
   int copy_to = -1;
   int working_size;
   int queue_oversize = 0;
   unsigned char temp;
   unsigned char bumped_char;

   while (marker < 2048 && copy_to == -1)
      {
         // make sure space is available
         get_queue_data (marker, &queue_id, &queue_size);

         if (queue_id == -1)
            {
               // Assume end of data; room to add to queue
               // If need to add new segment, do it here
               if (queue_oversize)
                  {
                     data[marker] = add_to_this;
                     // Shift id left one byte
                     data[marker] <<= 1;
                     data[marker] |= 0x80;
                     copy_to = marker;
                     queue_size = 0;
                  }
                  break;
            }

         if (queue_id == (int) add_to_this)
            copy_to = marker;

         if (queue_size == 511 && copy_to == marker)
            {
               // Another queue segment will be required
               // Insert new element, and bump 512th to next segment
               queue_oversize = 1;

               // Save "bumped" byte for next segment
               bumped_char = data[marker + 2 + 510];
               for (marker = copy_to + 510+2; marker > copy_to+2; marker--)
                  data[marker] = data[marker - 1];
               marker = copy_to;

               // Insert new element, and find a new place for bumped data
               data[marker + 2] = b;
               b = bumped_char;
               copy_to = -1;
            }

         marker += queue_size + 2;
      }

   if (marker >= 2048)
      {
         // Out of array space
         onOutOfMemory();
      }

   if (copy_to == -1)
      {
         // No such queue
         onIllegalOperation();
      }

   queue_size++;
   if (queue_size > 255)
      {
         data[copy_to] |= 0x01;
         data[copy_to + 1] = queue_size - 256;
      }
   else
      {
         data[copy_to] &= 0xFE;
         data[copy_to + 1] = queue_size;
      }         

   // Add an item to the start of this queue (past header bytes)
   copy_to += 2;

   // Move all following data down a byte
   for (marker = 2047; marker > copy_to; marker--)
      data[marker] = data[marker - 1];

   data[copy_to] = b;
}

// Dequeue and return a byte from specified queue
unsigned char deQ (Q *q)                     
{
   unsigned char get_from_this = (unsigned char) q;
   int marker = 0;
   int queue_id, queue_size;
   unsigned char return_this;
   int oversize_queue = 0;
   int target_pos = -1;
   int copy_from;

   while (marker < 2048 && target_pos == -1)
      {
         get_queue_data (marker, &queue_id, &queue_size);
         if (queue_id == -1)
            break;
         if (queue_id == get_from_this)
            {
               if (queue_size == 511)
                  {
                  // This queue is full, there could be another segment
                     oversize_queue = marker;
                     marker += queue_size + 2;
                  }
               else
                  {
                     target_pos = marker;
                  }
            }
         else
            {
                marker += queue_size + 2;
            }
      }

   if (queue_id == -1)
      {
         if (oversize_queue)
            {
               // No other segments found, remove from segment of size 511
               target_pos = oversize_queue;
               queue_size = 511;
            }
         else
            {
               printf ("Queue %d doesn't exist\n", get_from_this);
               // That queue doesn't exist
               onIllegalOperation();
            }
      }

   if (queue_size == 0)
      {
         printf ("Can't deQ empty queue %d\n", queue_id);
         // Can't de-queue an empty queue
         onIllegalOperation();
      }

   // Dequeue tail element
   queue_size--;

   marker = target_pos;               
   if (queue_size > 255)
      {
         data[marker] |= 0x01;
         data[marker + 1] = queue_size - 256;
      }
   else
      {
         data[marker] &= 0xFE;
         data[marker + 1] = queue_size;
      }

   marker += queue_size + 2;
   copy_from = marker + 1;
   return_this = data[marker];

   // Delete second queue segment if no elements left
   if (queue_size == 0 && oversize_queue)
      {
         copy_from = marker + 1;
         marker -= 2;
      }

   // Squeeze remaining data in one space

   while (copy_from <= 2047)
      data[marker++] = data[copy_from++];
   while (marker <= 2047)
      data[marker++] = 0;

   return return_this;
}