#include "Header_Files/sbgbuffer.h"

using namespace sbgtc;

rawBuffer sbgtc::initRawBuffer(QByteArray * sbgMessage)
{
    rawBuffer tmp;

    tmp.rxBuffer=(uint8_t *)sbgMessage->data();
    tmp.rxBufferSize=sbgMessage->size();

    return tmp;
}

SbgErrorCode sbgtc::sbgStreamBufferInitForRead(SbgStreamBuffer *pHandle, const void *pLinkedBuffer, size_t bufferSize)
{
    //
    // Check input parameters
    //
//    assert(pHandle);
//    assert(pLinkedBuffer);

    //
    // Initialize stream parameters
    //
    pHandle->bufferSize = bufferSize;
    pHandle->errorCode = SBG_NO_ERROR;

    //
    // Initialize the buffer
    //
    pHandle->pBufferPtr = (uint8_t*)pLinkedBuffer;
    pHandle->pCurrentPtr = (uint8_t*)pLinkedBuffer;

    //
    // For now, we don't handle any error, maybe we could add checks in debug mode only
    //
    return SBG_NO_ERROR;
}

SbgErrorCode sbgtc::sbgStreamBufferSeek(SbgStreamBuffer *pHandle, size_t offset, SbgSBSeekOrigin origin)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // According to the origin reference point
        //
        switch (origin)
        {
        case SB_SEEK_SET:
            pHandle->pCurrentPtr = pHandle->pBufferPtr + offset;
            break;
        case SB_SEEK_CUR_INC:
            pHandle->pCurrentPtr += offset;
            break;
        case SB_SEEK_CUR_DEC:
            pHandle->pCurrentPtr -= offset;
            break;
        case SB_SEEK_END:
            pHandle->pCurrentPtr = pHandle->pBufferPtr + (pHandle->bufferSize - offset);
            break;
        default:
            pHandle->errorCode = SBG_INVALID_PARAMETER;
            //SBG_LOG_ERROR(pHandle->errorCode, "Invalid origin parameter");
        }

        //
        // Make sure that no error has occurred
        //
        if (pHandle->errorCode == SBG_NO_ERROR)
        {
            //
            // Test if the current ptr is still within the buffer bounds
            //
            if (pHandle->pCurrentPtr < pHandle->pBufferPtr)
            {
                //
                // We are before the buffer so clamp to the begining of the buffer and raise an error
                //
                pHandle->pCurrentPtr = pHandle->pBufferPtr;
                pHandle->errorCode = SBG_BUFFER_OVERFLOW;

                //
                // Stream buffer underflow
                //
                //SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek before the buffer");
            }
            else if (pHandle->pCurrentPtr > pHandle->pBufferPtr + pHandle->bufferSize)
            {
                //
                // We are after the buffer so clamp to the end of the buffer and raise an error
                //
                pHandle->pCurrentPtr = pHandle->pBufferPtr + pHandle->bufferSize;
                pHandle->errorCode = SBG_BUFFER_OVERFLOW;

                //
                // Stream buffer overflow
                //
                //SBG_LOG_ERROR(pHandle->errorCode, "Trying to seek after the buffer");
            }
        }
    }

    return pHandle->errorCode;
}

size_t sbgtc::sbgStreamBufferTell(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    return (size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr;
}

void * sbgtc::sbgStreamBufferGetCursor(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    return pHandle->pCurrentPtr;
}

uint8_t sbgtc::sbgStreamBufferReadUint8LE(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint8_t))
        {
            //
            // Read the byte
            //
            return *((uint8_t*)(pHandle->pCurrentPtr++));
        }
        else
        {
            //
            // We have a buffer overflow
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0;
}

uint16_t sbgtc::sbgStreamBufferReadUint16LE(SbgStreamBuffer *pHandle)
{
    uint16_t bytesValues[2];

    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint16_t))
        {
            //
            // Test if the platform supports un-aligned access and if the endianness is the same
            //
            #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
                //
                // Read the current value
                //
                bytesValues[0] = *((uint16_t*)pHandle->pCurrentPtr);

                //
                //	Increment the current pointer
                //
                pHandle->pCurrentPtr += sizeof(uint16_t);

                return bytesValues[0];
            #else
                //
                // Read the each bytes
                //
                bytesValues[0] = *(pHandle->pCurrentPtr++);
                bytesValues[1] = *(pHandle->pCurrentPtr++);

                //
                // Store data according to platform endianness
                //
                #if (SBG_CONFIG_BIG_ENDIAN == 1)
                    return bytesValues[1] | (bytesValues[0] << 8);
                #else
                    return bytesValues[0] | (bytesValues[1] << 8);
                #endif
            #endif
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0;
}

uint32_t sbgtc::sbgStreamBufferReadUint32LE(SbgStreamBuffer *pHandle)
{
    uint32_t bytesValues[4];

    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint32_t))
        {
            //
            // Test if the platform supports un-aligned access and if the endianness is the same
            //
            #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
                //
                // Read the current value
                //
                bytesValues[0] = *((uint32_t*)pHandle->pCurrentPtr);

                //
                //	Increment the current pointer
                //
                pHandle->pCurrentPtr += sizeof(uint32_t);

                return bytesValues[0];
            #else
                //
                // Read the each bytes
                //
                bytesValues[0] = *(pHandle->pCurrentPtr++);
                bytesValues[1] = *(pHandle->pCurrentPtr++);
                bytesValues[2] = *(pHandle->pCurrentPtr++);
                bytesValues[3] = *(pHandle->pCurrentPtr++);

                //
                // Store data according to platform endianness
                //
                #if (SBG_CONFIG_BIG_ENDIAN == 1)
                    return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
                #else
                    return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
                #endif
            #endif
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0;
}

/*!
 * Read an uint64_t from a stream buffer (Little endian version).
 * \param[in]	pHandle				Valid stream buffer handle that supports read operations.
 * \return							The read value or 0 if we have an error.
 */
uint64_t sbgtc::sbgStreamBufferReadUint64LE(SbgStreamBuffer *pHandle)
{
    uint64_t lowPart;
    uint64_t highPart;

    //
    // Check input parameters
    //
//    assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(uint64_t))
        {
            //
            // Test if the platform supports un-aligned access and if the endianness is the same
            //
            #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
                //
                // Read the current value
                //
                lowPart = *((uint64_t*)pHandle->pCurrentPtr);

                //
                //	Increment the current pointer
                //
                pHandle->pCurrentPtr += sizeof(uint64_t);

                return lowPart;
            #else
                //
                // Read 64 bit value using two 32 bits read to avoid too much 64 bits operations
                //
                lowPart = sbgStreamBufferReadUint32LE(pHandle);
                highPart = sbgStreamBufferReadUint32LE(pHandle);

                //
                // Store data according to platform endianness
                //
                #if (SBG_CONFIG_BIG_ENDIAN == 1)
                    return (lowPart << 32) | highPart;
                #else
                    return lowPart | (highPart << 32);
                #endif
            #endif
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0ull;
}

int8_t sbgtc::sbgStreamBufferReadInt8LE(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int8_t))
        {
            //
            // Read the byte
            //
            return *((int8_t*)(pHandle->pCurrentPtr++));
        }
        else
        {
            //
            // We have a buffer overflow
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0;
}

int32_t sbgtc::sbgStreamBufferReadInt32LE(SbgStreamBuffer *pHandle)
{
    int32_t bytesValues[4];

    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(int32_t))
        {
            //
            // Test if the platform supports un-aligned access and if the endianness is the same
            //
            #if (SBG_CONFIG_UNALIGNED_ACCESS_AUTH == 1) && (SBG_CONFIG_BIG_ENDIAN == 0)
                //
                // Read the current value
                //
                bytesValues[0] = *((int32_t*)pHandle->pCurrentPtr);

                //
                //	Increment the current pointer
                //
                pHandle->pCurrentPtr += sizeof(int32_t);

                return bytesValues[0];
            #else
                //
                // Read the each bytes
                //
                bytesValues[0] = *(pHandle->pCurrentPtr++);
                bytesValues[1] = *(pHandle->pCurrentPtr++);
                bytesValues[2] = *(pHandle->pCurrentPtr++);
                bytesValues[3] = *(pHandle->pCurrentPtr++);

                //
                // Store data according to platform endianness
                //
                #if (SBG_CONFIG_BIG_ENDIAN == 1)
                    return bytesValues[3] | (bytesValues[2] << 8) | (bytesValues[1] << 16) | (bytesValues[0] << 24);
                #else
                    return bytesValues[0] | (bytesValues[1] << 8) | (bytesValues[2] << 16) | (bytesValues[3] << 24);
                #endif
            #endif
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0;
}

float sbgtc::sbgStreamBufferReadFloatLE(SbgStreamBuffer *pHandle)
{
    FloatNint floatInt;

    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(float))
        {
            //
            // Read the float as an uint32_t
            //
            floatInt.valU = sbgStreamBufferReadUint32LE(pHandle);

            //
            // Return the float using an union to avoid compiler cast
            //
            return floatInt.valF;
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0.0f;
}

/*!
 * Read an double from a stream buffer (Little endian version).
 * \param[in]	pHandle				Valid stream buffer handle that supports read operations.
 * \return							The read value or 0 if we have an error.
 */
double sbgtc::sbgStreamBufferReadDoubleLE(SbgStreamBuffer *pHandle)
{
    DoubleNint doubleInt;

    //
    // Check input parameters
    //
//    assert(pHandle);

    //
    // Test if we haven't already an error
    //
    if (pHandle->errorCode == SBG_NO_ERROR)
    {
        //
        // Test if we can access this item
        //
        if (sbgStreamBufferGetSpace(pHandle) >= sizeof(double))
        {
            //
            // Read the float as an uint64_t
            //
            doubleInt.valU = sbgStreamBufferReadUint64LE(pHandle);

            //
            // Return the double using an union to avoid compiler cast
            //
            return doubleInt.valF;
        }
        else
        {
            //
            // We have a buffer overflow so return 0
            //
            pHandle->errorCode = SBG_BUFFER_OVERFLOW;
        }
    }

    //
    // If we are here, it means we have an error so return 0
    //
    return 0.0;
}

SbgErrorCode sbgtc::sbgStreamBufferGetLastError(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Return error code
    //
    return pHandle->errorCode;
}

size_t sbgtc::sbgStreamBufferGetSpace(SbgStreamBuffer *pHandle)
{
    //
        // Check input parameters
        //
        //assert(pHandle);

        //
        // Return the space left in bytes
        //
        return sbgStreamBufferGetSize(pHandle) - sbgStreamBufferGetLength(pHandle);
}

size_t sbgtc::sbgStreamBufferGetSize(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Return the linked buffer size
    //
    return pHandle->bufferSize;
}

size_t sbgtc::sbgStreamBufferGetLength(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    //
    // Return the number of bytes between the begin of the stream and the current pointer
    //
    return ((size_t)pHandle->pCurrentPtr - (size_t)pHandle->pBufferPtr);
}

void * sbgtc::sbgStreamBufferGetLinkedBuffer(SbgStreamBuffer *pHandle)
{
    //
    // Check input parameters
    //
    //assert(pHandle);

    return pHandle->pBufferPtr;
}