/**
  ******************************************************************************
  * @file    stm_queue.c
  * @author  MCD Application Team
  * @brief   Queue management
  ******************************************************************************
   * @attention
  *
  * <h2><center>&copy; Copyright (c) 2019 STMicroelectronics. 
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the 
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
 */

/* Includes ------------------------------------------------------------------*/

/* Includes ------------------------------------------------------------------*/
#include "utilities_common.h"

#include "stm_queue.h"

/* Private define ------------------------------------------------------------*/
/* Private typedef -------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
#define MOD(X,Y) (((X) >= (Y)) ? ((X)-(Y)) : (X))

/* Private variables ---------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Extern variables ----------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/* Public functions ----------------------------------------------------------*/

/**
  * @brief   Initilaiilze queue strcuture .
  * @note   This function is used to initialize the global queue strcuture.  
  * @param  q: pointer on queue strcture to be initialised 
  * @param  queueBuffer: pointer on Queue Buffer
  * @param  queueSize:  Size of Queue Buffer
  * @param  elementSize: Size of an element in the queue. if =0, the queue will manage variable sizze elements
  * @retval   always 0
  */
int CircularQueue_Init(queue_t *q, uint8_t* queueBuffer, uint32_t queueSize, uint16_t elementSize, uint8_t optionFlags)
{
  q->qBuff = queueBuffer;
  q->first = 0;
  q->last = 0; /* queueSize-1; */
  q->byteCount = 0;
  q->elementCount = 0;
  q->queueMaxSize = queueSize;
  q->elementSize = elementSize;
  q->optionFlags = optionFlags;

   if ((optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG) && q-> elementSize)
   {
    /* can not deal with splitting at the end of buffer with fixed size element */
    return -1;
  }
  return 0;
}

/**
  * @brief   Add  element to the queue .
  * @note   This function is used to add one or more  element(s) to the Circular Queue .  
  * @param  q: pointer on queue structure   to be handled
  * @param  X; pointer on element(s) to be added 
  * @param  elementSize:  Size of element to be added to the queue. Only used if the queue manage variable size elements
  * @param  nbElements:  number of elements in the in buffer pointed by x
  * @retval  pointer on last element just added to the queue, NULL if the element to be added do not fit in the queue (too big)
  */
uint8_t* CircularQueue_Add(queue_t *q, uint8_t* x, uint16_t elementSize, uint32_t nbElements)
{

  uint8_t* ptr = NULL;                      /* fct return ptr to the element freshly added, if no room fct return NULL */
  uint16_t curElementSize = 0;              /* the size of the element currently  stored at q->last position */
  uint8_t  elemSizeStorageRoom  = 0 ;       /* Indicate the header (which contain only size) of element in case of varaibale size elemenet (q->elementsize == 0) */
  uint32_t curBuffPosition;                  /* the current position in the queue buffer */
  uint32_t i;                               /* loop counter */
  uint32_t NbBytesToCopy = 0, NbCopiedBytes = 0 ; /* Indicators for copying bytes in queue */
  uint32_t eob_free_size;                         /* Eof End of Quque Buffer Free Size */
  uint8_t  wrap_will_occur = 0;                   /* indicate if a wrap around will occurs */
  uint8_t  wrapped_element_eob_size;              /* In case of Wrap around, indicat size of parta of elemenet that fit at thened of the queuue  buffer */
  uint16_t overhead = 0;                          /* In case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG or CIRCULAR_QUEUE_NO_WRAP_FLAG options, 
                                                     indcate the size overhead that will be generated by adding the element with wrap management (split or no wrap ) */ 
  
  
  elemSizeStorageRoom  = (q->elementSize == 0) ? 2 : 0;
  /* retrieve the size of last element sored: the value stored at the beginning of the queue element if element size is variable otherwise take it from fixed element Size member */
  if (q->byteCount)
  {
    curElementSize = (q->elementSize == 0) ? q->qBuff[q->last] + ((q->qBuff[MOD((q->last+1), q->queueMaxSize)])<<8) + 2 : q->elementSize;
  }
  /* if queue element have fixed size , reset the elementSize arg with fixed element size value */
  if (q->elementSize > 0)               
  {
    elementSize = q->elementSize;
  }

   eob_free_size = (q->last >= q->first) ? q->queueMaxSize - (q->last + curElementSize) : 0;

   /* check how many bytes of wrapped element (if anay) are at end of buffer */
   wrapped_element_eob_size = (((elementSize + elemSizeStorageRoom )*nbElements) < eob_free_size) ? 0 : (eob_free_size % (elementSize + elemSizeStorageRoom));
   wrap_will_occur  = wrapped_element_eob_size > elemSizeStorageRoom;

   overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)) ? wrapped_element_eob_size : overhead;
   overhead = (wrap_will_occur && (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)) ? elemSizeStorageRoom  : overhead;
   
   
  /* Store now the elements if ennough room for all elements */
  if (elementSize && ((q->byteCount + ((elementSize + elemSizeStorageRoom )*nbElements) + overhead) <= q->queueMaxSize)) 
  { 
    /* loop to add all elements  */
    for (i=0; i < nbElements; i++) 
    {
      q->last = MOD ((q->last + curElementSize),q->queueMaxSize);
      curBuffPosition = q->last;
      
      /* store the element  */
      /* store fisrt the element size if element size is varaible */
      if (q->elementSize == 0) 
      {
        q->qBuff[curBuffPosition++]= elementSize & 0xFF;
        curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
        q->qBuff[curBuffPosition++]= (elementSize & 0xFF00) >> 8 ;
        curBuffPosition = MOD(curBuffPosition, q->queueMaxSize);
        q->byteCount += 2;
      }
      
      /* Identify number of bytes of copy takeing account possible wrap, in this case NbBytesToCopy will contains size that fit at end of the queue buffer */
      NbBytesToCopy = MIN((q->queueMaxSize-curBuffPosition),elementSize);
      /* check if no wrap (NbBytesToCopy == elementSize) or if Wrap and no spsicf option; 
         In thi case part of data will copied at the end of the buffer and the rest a the beggining */
      if ((NbBytesToCopy == elementSize) || ((NbBytesToCopy < elementSize) && (q->optionFlags == CIRCULAR_QUEUE_NO_FLAG)))
      {
        /* Copy First part (or emtire buffer ) from current position up to the end of the buffer queue (or before if enough room)  */
        memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
        /* Adjust bytes count */
        q->byteCount += NbBytesToCopy;
        /* Wrap */
        curBuffPosition = 0; 
        /* set NbCopiedBytes bytes with  ampount copied */
        NbCopiedBytes = NbBytesToCopy;
        /* set the rest to copy if wrao , if no wrap will be 0 */
        NbBytesToCopy = elementSize - NbBytesToCopy;
        /* set the current element Size, will be used to calaculate next last position at beggining of loop */
        curElementSize = (elementSize) + elemSizeStorageRoom ;
      }
      else if (NbBytesToCopy)  /* We have a wrap  to manage */
      {
       /* case of CIRCULAR_QUEUE_NO_WRAP_FLAG option */
         if (q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG)
        {
          /* if element size are variable and NO_WRAP option, Invalidate end of buffer setting 0xFFFF size*/
          if (q->elementSize == 0)
          {
             q->qBuff[curBuffPosition-2] = 0xFF;
             q->qBuff[curBuffPosition-1] = 0xFF;
          }
          q->byteCount += NbBytesToCopy;  /* invalid data at the end of buffer are take into account in byteCount */
          /* No bytes coped a the end of buffer */
          NbCopiedBytes = 0;
          /* all element to be copied at the begnning of buffer */
          NbBytesToCopy = elementSize; 
          /* Wrap */
          curBuffPosition = 0; 
          /* if variable size element, invalidate end of buffer setting OxFFFF in element header (size) */
          if (q->elementSize == 0)
          {
            q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
            q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
            q->byteCount += 2;   
          } 
           
        }
        /* case of CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG option */
        else if (q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG)
        {
          if (q->elementSize == 0)
          {
            /* reset the size of current element to the nb bytes fitting at the end of buffer */
             q->qBuff[curBuffPosition-2] = NbBytesToCopy & 0xFF;
             q->qBuff[curBuffPosition-1] = (NbBytesToCopy & 0xFF00) >> 8 ;
             /* copy the bytes */ 
             memcpy(&q->qBuff[curBuffPosition],&x[i*elementSize],NbBytesToCopy);
             q->byteCount += NbBytesToCopy; 
             /* set the number of copied bytes */
             NbCopiedBytes = NbBytesToCopy;             
             /* set rest of data to be copied to begnning of buffer */
             NbBytesToCopy = elementSize - NbBytesToCopy;
             /* one element more dur to split in 2 elements */
             q->elementCount++;
             /* Wrap */
             curBuffPosition = 0; 
             /* Set new size for rest of data */
             q->qBuff[curBuffPosition++] = NbBytesToCopy & 0xFF;
             q->qBuff[curBuffPosition++] = (NbBytesToCopy & 0xFF00) >> 8 ;
             q->byteCount += 2;              
          }
          else
          {
            /* Should not occur */
            /* can not manage split Flag on Fixed size element */
            /* Buffer is corrupted */
            return NULL;
          }
        }
        curElementSize = (NbBytesToCopy) + elemSizeStorageRoom ;
        q->last = 0;        
      }  
      
      /* some remaning byte to copy */
      if (NbBytesToCopy)      
      {
        memcpy(&q->qBuff[curBuffPosition],&x[(i*elementSize)+NbCopiedBytes],NbBytesToCopy);
        q->byteCount += NbBytesToCopy;
      }      
      
      /* One more element */
      q->elementCount++;
    }
    
    ptr = q->qBuff + (MOD((q->last+elemSizeStorageRoom ),q->queueMaxSize));
  }
  /* for Breakpoint only...to remove */
  else
  {
    return NULL;
  }
  return ptr;
}


/**
  * @brief  Remove element from  the queue and copy it in provided buffer
  * @note   This function is used to remove and element from  the Circular Queue .  
  * @param  q: pointer on queue structure  to be handled
  * @param  elementSize: Pointer to return Size of element to be removed  
  * @param  buffer: destination buffer where to copy element  
  * @retval Pointer on removed element. NULL if queue was empty
  */
uint8_t* CircularQueue_Remove_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
{
   return NULL;
}



/**
  * @brief  Remove element from  the queue.
  * @note   This function is used to remove and element from  the Circular Queue .  
  * @param  q: pointer on queue structure  to be handled
  * @param  elementSize: Pointer to return Size of element to be removed  
  * @retval Pointer on removed element. NULL if queue was empty
  */
uint8_t* CircularQueue_Remove(queue_t *q, uint16_t* elementSize)
{
  uint8_t  elemSizeStorageRoom = 0;
  uint8_t* ptr= NULL;
  elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
  *elementSize = 0;
  if (q->byteCount > 0) 
  {
    /* retreive element Size */
    *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;

     if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
     {
       if (((*elementSize == 0xFFFF) && q->elementSize == 0 ) || 
           ((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize))) 
       {
          /* all data from current position up to the end of buffer are invalid */
          q->byteCount -= (q->queueMaxSize - q->first);
          /* Adjust first element pos */
          q->first = 0;
          /* retrieve the rigth size after the wrap [if varaible size element] */
          *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
       }
     }

    /* retreive element */
    ptr = q->qBuff + (MOD((q->first + elemSizeStorageRoom), q->queueMaxSize));

    /* adjust byte count */
    q->byteCount -= (*elementSize + elemSizeStorageRoom) ;
    
    /* Adjust q->first */
    if (q->byteCount > 0)
    {
      q->first = MOD((q->first+ *elementSize + elemSizeStorageRoom ), q->queueMaxSize);
    }    
    /* adjust element count */    
    --q->elementCount;    
  }
  return ptr;
}


/**
  * @brief  "Sense" first element of the queue, without removing it and copy it in provided buffer
  * @note   This function is used to return a pointer on the first element of the queue without removing it.  
  * @param  q: pointer on queue structure  to be handled
  * @param  elementSize:  Pointer to return Size of element to be removed  
  * @param  buffer: destination buffer where to copy element
  * @retval Pointer on sensed element. NULL if queue was empty
  */

uint8_t* CircularQueue_Sense_Copy(queue_t *q, uint16_t* elementSize, uint8_t* buffer)
{
    return NULL;
}


/**
  * @brief  "Sense" first element of the queue, without removing it.
  * @note   This function is used to return a pointer on the first element of the queue without removing it.  
  * @param  q: pointer on queue structure  to be handled
  * @param  elementSize:  Pointer to return Size of element to be removed  
  * @retval Pointer on sensed element. NULL if queue was empty
  */
uint8_t* CircularQueue_Sense(queue_t *q, uint16_t* elementSize)
{
  uint8_t  elemSizeStorageRoom = 0;
  uint8_t* x= NULL;
  elemSizeStorageRoom = (q->elementSize == 0) ? 2 : 0;
  *elementSize = 0;
  uint32_t FirstElemetPos = 0;
    
  if (q->byteCount > 0) 
  {
    FirstElemetPos = q->first;
    *elementSize = (q->elementSize == 0) ? q->qBuff[q->first] + ((q->qBuff[MOD((q->first+1), q->queueMaxSize)])<<8) : q->elementSize;
    
    if ((q->optionFlags & CIRCULAR_QUEUE_NO_WRAP_FLAG) && !(q->optionFlags & CIRCULAR_QUEUE_SPLIT_IF_WRAPPING_FLAG))
    { 
      if (((*elementSize == 0xFFFF) && q->elementSize == 0 ) || 
          ((q->first > q->last) && q->elementSize && ((q->queueMaxSize - q->first) < q->elementSize))) 

      {
        /* all data from current position up to the end of buffer are invalid */
        FirstElemetPos = 0; /* wrap to the begiining of buffer */

        /* retrieve the rigth size after the wrap [if varaible size element] */
        *elementSize = (q->elementSize == 0) ? q->qBuff[FirstElemetPos]+ ((q->qBuff[MOD((FirstElemetPos+1), q->queueMaxSize)])<<8) : q->elementSize;
      }
   }
   /* retrieve element */
    x = q->qBuff + (MOD((FirstElemetPos + elemSizeStorageRoom), q->queueMaxSize));
  }
  return x;
}

/**
  * @brief   Check if queue is empty.
  * @note    This function is used to to check if the queue is empty.  
  * @param  q: pointer on queue structure  to be handled
  * @retval   TRUE (!0) if the queue is empyu otherwise FALSE (0)
  */
int CircularQueue_Empty(queue_t *q)
{
  int ret=FALSE;
  if (q->byteCount <= 0) 
  {
    ret=TRUE;
  } 
  return ret;
}

int CircularQueue_NbElement(queue_t *q)
{
  return q->elementCount;
}