/**************************************************************************
 *
 *  Copyright (c) 2019-2019 Diality Inc. - All Rights Reserved.
 *
 *  THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
 *  WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
 *
 *  @file MsgQueues.c
 *
 *  @date 08-Oct-2019
 *  @author S. Nash
 *
 *  @brief MsgQueues service module.  Provides message queue functionality.
 *
 **************************************************************************/

#include "Common.h"
#include "MsgQueues.h"

// ********** private definitions **********

#define MAX_MSG_QUEUE_SIZE          25      // bytes

// ********** private data **********

static U32 msgQueueCounts[NUM_OF_MSG_QUEUES];
static U32 msgQueueStarts[NUM_OF_MSG_QUEUES];
static U32 msgQueueNexts[NUM_OF_MSG_QUEUES];
static MESSAGE_WRAPPER_T msgQueues[NUM_OF_MSG_QUEUES][MAX_MSG_QUEUE_SIZE];

// ********** private function prototypes **********

/*************************************************************************
 * @brief initMsgQueues
 * The initMsgQueues function initializes the MsgQueues module.
 * @details
 * Inputs : none
 * Outputs : none
 * @param none
 * @return none
 *************************************************************************/
void initMsgQueues( void )
{
    U32 q, m;

    // reset message queues
    for ( q = 0; q < NUM_OF_MSG_QUEUES; q++ )
    {
        msgQueueCounts[q] = 0;
        msgQueueStarts[q] = 0;
        msgQueueNexts[q] = 0;
        for ( m = 0; m < MAX_MSG_QUEUE_SIZE; m++ )
        {
            blankMessageInWrapper( &msgQueues[q][m] );
        }
    }
}

/*************************************************************************
 * @brief addToMsgQueue
 * The addToMsgQueue function adds a message to a given message queue. \n
 * A CRC is calculated for the message and included in the message wrapper.
 * @details
 * Inputs : none
 * Outputs : none
 * @param queue : the message queue to add to
 * @param msg : a pointer to a message structure to add to the queue
 * @return TRUE if message added to queue, FALSE if could not
 *************************************************************************/
BOOL addToMsgQueue( MSG_QUEUE_T queue, MESSAGE_WRAPPER_T *msg )
{
    BOOL result = FALSE;

    // verify given message queue
    if ( queue < NUM_OF_MSG_QUEUES )
    {
        U32 next;
        U32 count;
        BOOL full = FALSE;

        // thread protection
        // TODO - disable interrupts (selectively if possible)

        // reserve next available queue space while under thread protection
        next = msgQueueNexts[queue];
        count = msgQueueCounts[queue];
        if ( count < MAX_MSG_QUEUE_SIZE )
        {   // increment queue count and next queue indexes while under thread protection
            msgQueueCounts[queue]++;
            msgQueueNexts[queue] = INC_WRAP(next,0,MAX_MSG_QUEUE_SIZE);
        }
        else
        {
            full = TRUE;
        }

        // end thread protection
        // TODO - re-enable interrupts

        // make sure queue is not full
        if ( FALSE == full )
        {
            // add message to queue
            msgQueues[queue][next] = *msg;

            // TODO - calculate CRC for given message (just zero for now)
            msgQueues[queue][next].crc = 0x0;
        }
        else // message queue is full
        {
            // TODO - s/w fault?
        }
    }
    else // invalid message queue
    {
        // TODO - s/w fault
    }

    return result;
}

/*************************************************************************
 * @brief getFromMsgQueue
 * The getFromMsgQueue function retrieves the next message from a given \n
 * message queue.
 * @details
 * Inputs : none
 * Outputs : none
 * @param queue : the message queue to retrieve from
 * @param msg : a pointer to a message structure to stuff
 * @return TRUE if a message was found to retrieve, FALSE if not
 *************************************************************************/
BOOL getFromMsgQueue( MSG_QUEUE_T queue, MESSAGE_WRAPPER_T *msg )
{
    BOOL result = FALSE;

    // verify given message queue
    if ( queue < NUM_OF_MSG_QUEUES )
    {
        U32 first;
        U32 count;
        BOOL empty = FALSE;

        // thread protection
        // TODO - disable interrupts (selectively if possible)

        // reserve next available queue space while under thread protection
        first = msgQueueStarts[queue];
        count = msgQueueCounts[queue];
        if ( count > 0 )
        {   // decrement queue count and increment start queue index while under thread protection
            msgQueueCounts[queue]--;
            msgQueueStarts[queue] = INC_WRAP(first,0,MAX_MSG_QUEUE_SIZE);
        }
        else
        {
            empty = TRUE;
        }

        // end thread protection
        // TODO - re-enable interrupts

        // make sure queue is not empty
        if ( FALSE == empty )
        {
            // get message from queue
            *msg = msgQueues[queue][first];

            result = TRUE;
        }
        else // message queue is full
        {
            // TODO - s/w fault?
        }
    }
    else // invalid message queue
    {
        // TODO - s/w fault
    }

    return result;
}

/*************************************************************************
 * @brief isMsgQueueEmpty
 * The isMsgQueueEmpty function determines whether a given message queue is empty.
 * @details
 * Inputs : none
 * Outputs : none
 * @param queue : the message queue to check
 * @return TRUE if a given message queue is empty, FALSE if not
 *************************************************************************/
BOOL isMsgQueueEmpty( MSG_QUEUE_T queue )
{
    BOOL result = FALSE;

    // verify given message queue
    if ( queue < NUM_OF_MSG_QUEUES )
    {
        if ( msgQueueCounts[queue] == 0 )
        {
            result = TRUE;
        }
    }
    else // invalid message queue
    {
        // TODO - s/w fault
    }

    return result;
}

/*************************************************************************
 * @brief isMsgQueueFull
 * The isMsgQueueFull function determines whether a given message queue is full.
 * @details
 * Inputs : none
 * Outputs : none
 * @param queue : the message queue to check
 * @return TRUE if the given message queue is full, FALSE if not
 *************************************************************************/
BOOL isMsgQueueFull( MSG_QUEUE_T queue )
{
    BOOL result = TRUE;

    // verify given message queue
    if ( queue < NUM_OF_MSG_QUEUES )
    {
        if ( msgQueueCounts[queue] < MAX_MSG_QUEUE_SIZE )
        {
            result = FALSE;
        }
    }
    else // invalid message queue
    {
        // TODO - s/w fault
    }

    return result;
}

/*************************************************************************
 * @brief blankMessage
 * The blankMessage function blanks a given message.
 * @details
 * Inputs : none
 * Outputs : none
 * @param message : Pointer to the message to blank.
 * @return none
 *************************************************************************/
void blankMessage( MESSAGE_T *message )
{
    U32 i;
    U32 msgSize = sizeof(MESSAGE_T);
    U08 *msgContent = (U08*)message;

    // zero out the message
    for ( i = 0; i < msgSize; i++ )
    {
        *msgContent++ = 0x0;
    }

    // set msg ID out of bounds in case blank message goes somewhere
    message->hdr.msgID = 0xFFFF;
}

/*************************************************************************
 * @brief blankMessageInWrapper
 * The blankMessageInWrapper function blanks a given message in a wrapper.
 * @details
 * Inputs : none
 * Outputs : none
 * @param message : Pointer to the message in a wrapper to blank.
 * @return none
 *************************************************************************/
void blankMessageInWrapper( MESSAGE_WRAPPER_T *message )
{
    U32 i;
    U32 msgSize = sizeof(MESSAGE_T);
    U08 *msgContent = (U08*)message;

    // zero out the message
    for ( i = 0; i < msgSize; i++ )
    {
        *msgContent++ = 0x0;
    }

    // set msg ID out of bounds in case blank message goes somewhere
    message->msg.hdr.msgID = 0xFFFF;
}