#include "buffer_queue.h"


QUEUE_T g_queue;

/* receive queue buffer */
UCHAR g_send_queue[BUFFER_SIZE];

/* Length of data currently held in queue */
#define QUEUE_LENGTH() \
  ((g_queue.g_tail >= g_queue.g_head) ? \
  g_queue.g_tail - g_queue.g_head \
  : BUFFER_SIZE - g_queue.g_head + g_queue.g_tail)

#define QUEUE_FREE() \
  ((g_queue.g_tail >= g_queue.g_head) ? \
  BUFFER_LEN - g_queue.g_tail + g_queue.g_head \
  : g_queue.g_head - g_queue.g_tail - 1)


static void copyIntoBuffer(const CHAR *p_data, USHORT len)
{
    /* Sanity check */
    if ((len == 0) || (p_data == NULL))
        return;

    /* No point copying more data into the queue than the queue can hold */
    if (len > BUFFER_LEN)
    {
        /* Advance input pointer to the last BUFFER_LEN bytes */
        p_data += len - BUFFER_LEN;

        /* Adjust len */
        len = BUFFER_LEN;
    }

    /* Check whether the queue will overflow */
    if (len > QUEUE_FREE())
    {
        /* Advance g_head to point to the oldest item, after the overflow*/
        g_queue.g_head += len - QUEUE_FREE();

        /* If this goes past the end of the buffer, wrap around */
        if (g_queue.g_head >= BUFFER_SIZE)
            g_queue.g_head -= BUFFER_SIZE;

        /* Update g_peek similarly */
        g_queue.g_peek = g_queue.g_head;
    }

    /* Check whether we're going past the end of the buffer */
    if (g_queue.g_tail + len >= BUFFER_SIZE)
    {
        /* Calculate how much space there is till the end of the buffer */
        const USHORT available = BUFFER_SIZE - g_queue.g_tail;

        /* Copy data into the queue up to end of buffer */
        memcpy(&g_send_queue[g_queue.g_tail], p_data, available);

        /* Update g_tail */
        g_queue.g_tail = len - available;

        /* Copy data into the queue up to end of buffer */

        memcpy(g_send_queue, p_data + available, g_queue.g_tail);

    }
    else
    {
        /* Copy data into the queue up to end of buffer */
           memcpy(&g_send_queue[g_queue.g_tail], p_data, len);

        /* Update g_tail */
        g_queue.g_tail += len;
    }
}


static USHORT peekBuffer(CHAR *p_data, USHORT len)
{
	USHORT peeked = len;    /* Number of bytes of data peeked */

    /* Sanity check */
    if ((len == 0) || (p_data == NULL))
        return 0;

    /* Cannot peek more data than is available */
    if (peeked > QUEUE_LENGTH())
    {
        peeked = QUEUE_LENGTH();
        
        if(peeked>len)
            peeked = len;
    }

    /* Check whether we're going past the end of the buffer */
    if (g_queue.g_head + peeked >= BUFFER_SIZE)
    {
        /* Calculate how much space there is till the end of the buffer */
        const USHORT available = BUFFER_SIZE - g_queue.g_head;
        

        if(available>len)
        {
            /* Copy data up to end of buffer */
            memcpy(p_data, &g_send_queue[g_queue.g_head], len);
        }
        else
        {
            /* Copy data up to end of buffer */
            memcpy(p_data, &g_send_queue[g_queue.g_head], available);
        }

        /* Update g_peek */
        g_queue.g_peek = peeked - available;

        /* Copy data from start of buffer */ 
        memcpy(p_data + available,g_send_queue, g_queue.g_peek);

    }
    else
    {
        /* Peek data starting from the head of the queue */
        memcpy(p_data, &g_send_queue[g_queue.g_head], peeked);

        /* Update g_peek */
        g_queue.g_peek = g_queue.g_head + peeked;
    }

    return peeked;
}

static void removeDataOut(void)
{
    /* Update g_head to point to current g_peek location */
    g_queue.g_head = g_queue.g_peek;
}


void buffer_queue_PushBytes(CHAR *p_data, USHORT len)
{
    /* Copy data into the buffer whether or not space is available */
    copyIntoBuffer(p_data, len);
}

USHORT buffer_queue_PopBytes(CHAR *p_data, USHORT len)	////output data, and remove
{
    /* Copy the data into the return buffer */
    USHORT peeked = peekBuffer(p_data,len);

    /* Remove the peeked data from the queue */
    removeDataOut();

    /* Return number of bytes peeked */
    return peeked;
}


void buffer_queue_removePeekBytes(void)
{
	removeDataOut();
}

USHORT buffer_queue_PeekBytes(CHAR *p_data, USHORT len)	////output buffer, but not remove
{
    /* Peek into the buffer */
    return peekBuffer(p_data, len);
}


USHORT buffer_queue_GetBufferCapacity(void)
{
    return BUFFER_LEN;
}


USHORT buffer_queue_GetDataSize(void)
{
    return QUEUE_LENGTH();
}

USHORT buffer_queue_GetAvailableSize(void)
{
    return QUEUE_FREE();
}

void buffer_queue_ClearBuffer(void)
{
	 /* Reset queue pointers */
	 g_queue.g_head=g_queue.g_peek=g_queue.g_tail = 0;
}

/*
void buffer_queue_InitTestBuffer(void)
{
	USHORT i = 0;
	UCHAR k = 0;
	for(i=0; i<BUFFER_SIZE; i++)
	{
		k = i&0xff;
		copyIntoBuffer(&k, 1);
	}
}
*/

