/*
 *  Copyright 2009 OpenSourceStewardshipFoundation.org
 *  Licensed under GNU General Public License version 2
 *
 * NOTE: this version of SRSW correct as of April 25, 2010
 *
 * Author: seanhalle@yahoo.com
 */


#include <string.h>
#include <stdlib.h>
#include <stdio.h>

#include "PrivateQueue.h"



//===========================================================================

/*This kind of queue is private to a single core at a time -- has no
 * synchronizations 
 */

PrivQueueStruc* makePrivQ()
 {
   PrivQueueStruc* retQ;
      //This malloc is not safe to use in wrapper lib nor app code!
   retQ = (PrivQueueStruc *) VMS_int__malloc( sizeof( PrivQueueStruc ) );

      //This malloc is not safe to use in wrapper lib nor app code!
   retQ->startOfData = VMS_int__malloc( 1024 * sizeof(void *) );
   memset( retQ->startOfData, 0, 1024 * sizeof(void *) );
   retQ->extractPos = &(retQ->startOfData[0]); //side by side == empty
   retQ->insertPos  = &(retQ->startOfData[1]); // so start pos's have to be
   retQ->endOfData  = &(retQ->startOfData[1023]);

   #ifdef DEBUG_PRIVATE_Q
   retQ->numReads = 0;
   retQ->numWrites =0;
   #endif
   
   return retQ;
 }


void
enlargePrivQ( PrivQueueStruc *Q )
 { size_t  oldSize, newSize, topPartSize, bottPartSize;
   char *insertPos, *extractPos;
   char *oldStartOfData, *oldEndOfData, *newStartOfData, *newEndOfData;
   size_t  insertOffsetBytes, extractOffsetBytes;
   char*  copyStartAddr;

   #ifdef DEBUG_PRIVATE_Q
   printf("Enlarging queue Q = %p\nnumReads = %d; numWrites = %d\n",Q,Q->numReads,Q->numWrites);
   #endif
   
   oldStartOfData = (char*)Q->startOfData;
   oldEndOfData   = (char*)(Q->endOfData + 1);
   insertPos      = (char*)Q->insertPos;
   extractPos     = (char*)Q->extractPos;
   
      //TODO: verify these get number of bytes correct
   insertOffsetBytes  = (insertPos  - oldStartOfData);
   extractOffsetBytes = (extractPos - oldStartOfData);
   
   oldSize            = oldEndOfData - oldStartOfData; //in bytes
   newSize            = 2 * oldSize;
   
   #ifdef DEBUG_PRIVATE_Q
   printf("Old size = %d, new size = %d\n",(int)oldSize,(int)newSize);
   #endif
   
      //This malloc is not safe to use in wrapper lib nor app code!
   newStartOfData     = (char *) VMS_int__malloc( newSize );
   if(newStartOfData == NULL){
       perror("malloc"); exit(1);
   }
   newEndOfData       = newStartOfData + newSize; //all calcs in Bytes

      //TODO: test all of this, for both cases
   Q->startOfData     = newStartOfData;
   Q->endOfData       = ((void **)newEndOfData) - 1;
   
      //Moving the data and pointers to the new array is
      //a little trickier than at first it seems..  the top part
      // of old queue must be moved to the top part of new queue, while
      // bottom part of old to bottom part of new, then the new insert
      // and extract positions calculated by offset from top and bottom
      //UNLESS the one case where old extract was at bottom and insert
      // was at top.
      //TODO: check that this is correct!
   if( extractPos == oldStartOfData )
    {
      memcpy( newStartOfData, oldStartOfData, oldSize ); //oldSize is bytes
      Q->extractPos  = Q->startOfData; //start of valid data
      Q->insertPos   = (void**)(newStartOfData + insertOffsetBytes); //end of valid data
    }
   else //have to copy two parts separately, then calc positions
    {    //TODO: check end-addr, sizes, and new positions carefully
       
         //copy top part, starting at extract up until end of data,
         // into top of new array
      topPartSize = oldEndOfData - extractPos;
      copyStartAddr = newEndOfData - topPartSize;
      memcpy( copyStartAddr, extractPos, topPartSize );
      Q->extractPos = (void **)copyStartAddr; //extract just-copied data
  
         //copy bottom part, from old start up to old insert,
         // into bottom of new array		 
      bottPartSize = insertPos - oldStartOfData; //-1 for empty insertPos
      memcpy( newStartOfData, oldStartOfData, bottPartSize );
      Q->insertPos  = (void **)(newStartOfData + bottPartSize);
    }
      //This free is not safe to use in wrapper lib nor app code!
   VMS_int__free(oldStartOfData);    
 }


/*Returns TRUE when queue is empty
 */
bool32 isEmptyPrivQ( PrivQueueStruc* Q )
 { void *out    = 0;
   void **startOfData = Q->startOfData;
   void **endOfData   = Q->endOfData;

   void **insertPos  = Q->insertPos;
   void **extractPos = Q->extractPos;

      //if not empty -- (extract is just below insert when empty)
   if( insertPos - extractPos != 1 &&
       !(extractPos == endOfData && insertPos == startOfData))
    { 
      return FALSE;
    }
      //Q is empty
   return TRUE;
 }

/*Returns NULL when queue is empty
 */
void* peekPrivQ( PrivQueueStruc* Q )
 { void *out    = 0;
   void **startOfData = Q->startOfData;
   void **endOfData   = Q->endOfData;

   void **insertPos  = Q->insertPos;
   void **extractPos = Q->extractPos;

      //if not empty -- (extract is just below insert when empty)
   if( insertPos - extractPos != 1 &&
       !(extractPos == endOfData && insertPos == startOfData))
    { 
      out = *(Q->extractPos + 1);
      return out;
    }
      //Q is empty
   return NULL;
 }

/*Returns NULL when queue is empty
 */
void* readPrivQ(PrivQueueStruc* Q) {
#ifdef DEBUG_PRIVATE_Q
    Q->numReads++;
#endif
    
    void *out = 0;
    void **startOfData = Q->startOfData;
    void **endOfData = Q->endOfData;

    void **insertPos = Q->insertPos;
    void **extractPos = Q->extractPos;

    //if not empty -- (extract is just below insert when empty)
    if (insertPos - extractPos != 1 &&
            !(extractPos == endOfData && insertPos == startOfData)) { //move before read
        if (extractPos == endOfData) //write new pos exactly once, correctly
        {
            Q->extractPos = startOfData; //can't overrun then fix it 'cause
        }// other thread might read bad pos
        else {
            Q->extractPos++;
        }
        out = *(Q->extractPos);
        return out;
    }
    //Q is empty
    return NULL;
}
/*Expands the queue size automatically when it's full
 */
void
writePrivQ(void * in, PrivQueueStruc* Q) {

    #ifdef DEBUG_PRIVATE_Q
    Q->numWrites++;
    #endif
    
    //tryAgain:
    //Full? (insert is just below extract when full)
    if ((Q->extractPos - Q->insertPos) == 1 ||
            (Q->insertPos == Q->endOfData && Q->extractPos == Q->startOfData)) {
        enlargePrivQ(Q);
    }

    *(Q->insertPos) = in; //insert before move
    if (Q->insertPos == Q->endOfData) //write new pos exactly once, correctly
    {
        Q->insertPos = Q->startOfData;
    } else {
        Q->insertPos++;
    }
    return;

    //Q is full

    //goto tryAgain;
}


/*Returns false when the queue was full.
 * have option of calling make_larger_PrivQ to make more room, then try again
 */
bool32
writeIfSpacePrivQ( void * in, PrivQueueStruc* Q )
 {
   void **startOfData = Q->startOfData;
   void **endOfData   = Q->endOfData;

   void **insertPos  = Q->insertPos;
   void **extractPos = Q->extractPos;

   if( extractPos - insertPos != 1 &&
       !(insertPos == endOfData && extractPos == startOfData))
    { *(Q->insertPos) = in;   //insert before move
      if( insertPos == endOfData ) //write new pos exactly once, correctly
       { Q->insertPos = startOfData;
       }
      else
       { Q->insertPos++;
       }
      #ifdef DEBUG_PRIVATE_Q
      Q->numWrites++;
      #endif
      return TRUE;
    }
      //Q is full
   return FALSE;
 }

int32
numInPrivQ( PrivQueueStruc *Q )
 { int32 size, numIn;
 
   if( Q->insertPos < Q->extractPos )
    {    //insert has wrapped around so numIn is:
         // insertPos + size - extractPos -- Consider, is empty when
         // extractPos = endOfData and insert = start -- correctly get zero
      size  = Q->endOfData - Q->startOfData + 1; //sz of 10 is 0..9
      numIn = Q->insertPos - Q->extractPos + size - 1; //-1 bec insrt empty
    }
   else
    {
      numIn =  Q->insertPos -  Q->extractPos -1;//-1 bec insertPos empty
    }
   return numIn;
 }


/*Treats queue as a stack -- no matter contents, if read done right after
 * a push, then the pushed item is what comes out.
 * Expands the queue size automatically when it's full.
 */
void
pushPrivQ( void * in, PrivQueueStruc* Q )
 {
    #ifdef DEBUG_PRIVATE_Q
    Q->numWrites++;
    #endif
   while(1){
       void **startOfData = Q->startOfData;
       void **endOfData   = Q->endOfData;

       void **insertPos  = Q->insertPos;
       void **extractPos = Q->extractPos;

          //Full? (insert is just below extract when full)
       if( extractPos - insertPos != 1 &&
           !(insertPos == endOfData && extractPos == startOfData))
        {    //insert -- but go backwards, inserting at read position then
             // move read pos backwards
          *(Q->extractPos) = in;
          if( extractPos == startOfData ) //write new pos exactly once, correctly
           { Q->extractPos = endOfData;   //can't overrun then fix it 'cause
           }                              // other thread might read bad pos
          else
           { Q->extractPos--;
           }
          return;
        }
          //Q is full
       enlargePrivQ( Q );
   }
 }


 void
 freePrivQ( PrivQueueStruc *Q )
  {
      //This free is not safe to use in wrapper lib nor app code!
    VMS_int__free( Q->startOfData );
	VMS_int__free( Q );
  }