#include "MesgQueue.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <assert.h>

MesgQueue* MesgQueue::s_queueCollection[COLS_NUM];
int MesgQueue::s_cCols=0;

MesgQueue * MesgQueue::GetQueue( int id ) 
{
   for( int i = 0; i < s_cCols; i++ )
      if ( s_queueCollection[i]->id() == id )   // Found an existing queue with this id.
         return s_queueCollection[i];

   MesgQueue * newQueue = new MesgQueue( id );
   if( s_cCols >= COLS_NUM )
   {
      fprintf( stderr, "No more MsgQueue collections available\n" );
      exit( -1 );
   }
   s_queueCollection[s_cCols] = newQueue ;
   ++s_cCols;

   return newQueue;
}

void MesgQueue::Fini()
{
   for( int i = 0; i < s_cCols; i++ )
      delete s_queueCollection[s_cCols];
}

MesgQueue::MesgQueue( int id )
: _id( id )
, buffer( NULL )
, readIndex( 0 )
, writeIndex( 0 )
{ 
   buffer = new char[QUEUE_SIZE+2];
   pthread_mutex_init( &sharedMutex, NULL );
}


MesgQueue::~MesgQueue()
{
   delete[] buffer;
   pthread_mutex_destroy( &sharedMutex ); 
}


bool MesgQueue::IsDataAvailable( void ) 
{
   return readIndex != writeIndex;
}

void MesgQueue::Send( CMsg& msg )
{
   char* msgBuffer = NULL;
   int cBytes = msg.GetRawMsg( msgBuffer );
   //printf(" cBytes %d; buffer %x\n", cBytes, msgBuffer );
   assert( cBytes > 0 && msgBuffer != NULL );
   //printf("msgBUffer %d,%d,%d,%d\n", msgBuffer[0], msgBuffer[1], msgBuffer[2], msgBuffer[3] );

   //int oldWI = writeIndex;
   WriteBytes( msgBuffer, cBytes );
   //printf("afterSend: %d,%d,%d,%d\n", buffer[oldWI], buffer[oldWI+1], buffer[oldWI+2], buffer[oldWI+3] );
}

CMappedMsg MesgQueue::GetMsg()
{
   int effecReadIndex = readIndex%QUEUE_SIZE;
   // printf(" rI=%d, wI=%d\n", effecReadIndex, writeIndex );
   assert( effecReadIndex != writeIndex%QUEUE_SIZE );

   short cMsgSize = ntohs(*(const short *)(&buffer[effecReadIndex]));
   //short cMsgSize = *(const short *)(&buffer[effecReadIndex]);
   bool  fWrapAround = false;

   if( cMsgSize == -1 )
   {
      fWrapAround = true;
      effecReadIndex = 0;
      cMsgSize = ntohs(*(const short *)(&buffer[effecReadIndex]));
      //cMsgSize = *(const short *)(&buffer[effecReadIndex]);
   }

   // printf("cMsgSize=%d, %d,%d,%d,%d\n", cMsgSize, 
   //         buffer[effecReadIndex],   buffer[effecReadIndex+1], 
   //         buffer[effecReadIndex+2], buffer[effecReadIndex+3] );
   assert( cMsgSize >= 4 && (effecReadIndex+cMsgSize) <= QUEUE_SIZE );

   CMappedMsg msg;
   msg.SetMsg( &buffer[effecReadIndex], cMsgSize );
   if( !fWrapAround )
      readIndex += cMsgSize;
   else
      readIndex = cMsgSize;

   return msg;
}


void
MesgQueue::WriteBytes( const char * data, int size ) 
{
   bool fWriteWrapAround = false;
   int effecWriteIndex = writeIndex%QUEUE_SIZE;
   int effecReadIndex = readIndex%QUEUE_SIZE;

   if( ( effecWriteIndex + size )  > QUEUE_SIZE  )
   {  // ensure we don't break up messages in the buffer   
      fWriteWrapAround = true;
      // write special message to indicate the the flow of data 
      // switched to the beginning of the buffer
      char special[2];
      char* szSpec = &special[0];
      short spChar = -1;
      *(short *)szSpec = htons( spChar );
      //*(short *)szSpec = spChar;
      memcpy( &buffer[effecWriteIndex], szSpec, 2 );
      effecWriteIndex = 0;
   }

   if( effecReadIndex > effecWriteIndex && effecWriteIndex + size > effecReadIndex )
   {
      fprintf( stderr, "Error in MesgQueue::WriteBytes: overwriting unread data.\n" );
      exit( EXIT_FAILURE );
   }

   memcpy( &buffer[effecWriteIndex], data, size );
   if( !fWriteWrapAround )
      writeIndex += size;
   else
      writeIndex = size;
}

void
MesgQueue::ReadBytes( char * buf, int size ) 
{
   while ( writeIndex - readIndex < size )
      usleep( 1 );    // Wait until there is enough data.
   memcpy( buf, &buffer[readIndex % QUEUE_SIZE], size );
   readIndex += size;
}
