#include <string.h>

#include "tlv_parser.h"
#include "dbg.h"


//In    tlvData
//In    tlvDataLen
//return    tag
uint32 getTag(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    
    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < TAG_SIZE || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    return *((uint32*)(pBlobInput->data + offset));
}


//In    tlvData
//In    tlvDataLen
//return    length
uint32 getLength(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    uint32 length = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE + LENGTH_SIZE) || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE + LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + (TAG_SIZE + LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    length = *((uint32*)(pBlobInput->data + offset + TAG_SIZE));

    if (pBlobInput->length < uint64Offset + TAG_SIZE + LENGTH_SIZE + length) {
        printD("input is invalid");
        return 0;
    }

    return length;
}


//In    tlvData
//In    tlvDataLen
//return    tag
uint32 getTagBE(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    uint32 tag = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < TAG_SIZE || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + TAG_SIZE) {
        printD("input is invalid");
        return 0;
    }

    tag = *(pBlobInput->data + offset);
    tag <<= 8;
    tag |= *(pBlobInput->data + offset + 1);

    return tag;
}


//In    tlvData
//In    tlvDataLen
//return    length
uint32 getLengthBE(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    uint32 length = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE + LENGTH_SIZE) || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE + LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + (TAG_SIZE + LENGTH_SIZE)) {
        printD("input is invalid");
        return 0;
    }

    length = *(pBlobInput->data + offset + TAG_SIZE);
    length <<= 8;
    length |= *(pBlobInput->data + offset + TAG_SIZE + 1);

    if (pBlobInput->length < uint64Offset + TAG_SIZE + LENGTH_SIZE + length) {
        printD("input is invalid");
        return 0;
    }

    return length;
}


//In    tlvData
//In    tlvDataLen
//return    tag
uint32 getTag2ByteBE(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    uint32 tag = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < TAG_SIZE_2BYTE || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - TAG_SIZE_2BYTE) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + TAG_SIZE_2BYTE) {
        printD("input is invalid");
        return 0;
    }

    tag = *(pBlobInput->data + offset);
    tag <<= 8;
    tag |= *(pBlobInput->data + offset + 1);

    return tag;
}


//In    tlvData
//In    tlvDataLen
//return    length
uint32 getLength2ByteBE(const BlobData* pBlobInput, const uint32 offset)
{
    uint64 uint64Offset = offset;
    uint32 length = 0;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE) || pBlobInput->length > UINT32_MAX) {
        printD("input is invalid");
        return 0;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE)) {
        printD("input is invalid");
        return 0;
    }

    if(pBlobInput->length < uint64Offset + (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE)) {
        printD("input is invalid");
        return 0;
    }

    length = *(pBlobInput->data + offset + TAG_SIZE_2BYTE);
    length <<= 8;
    length |= *(pBlobInput->data + offset + TAG_SIZE_2BYTE + 1);

    if (pBlobInput->length < uint64Offset + TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE + length) {
        printD("input is invalid");
        return 0;
    }

    return length;
}


//In    pBlobInput
//In    targetData
//Out   outputParam
//return    boolean
boolean getValue(const BlobData* pBlobInput, const TlvDecodeData targetData, BlobData* pBlobOutput)
{
    CHECK_BLOB_POINTER(pBlobInput, FALSE);
    CHECK_BLOB_POINTER(pBlobOutput, FALSE);

    if(pBlobOutput->dataSize < targetData.length) {
        printE("buffer too small");
        return FALSE;
    }

    if (pBlobInput->length < targetData.offset + targetData.length) {
        printE("invalid buffer area");
        return FALSE;
    }

    pBlobOutput->length = targetData.length;
    memcpy(pBlobOutput->data, pBlobInput->data + targetData.offset, pBlobOutput->length);

    return TRUE;
}


//In    pBlobInput
//In    targetData
//Out   outputParam
//return    boolean
boolean getValue_uint8(const BlobData* pBlobInput, const TlvDecodeData targetData, uint8* pTempUint8)
{
    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pTempUint8 == NULL) {
        printE("output buffer is NULL");
        return FALSE;
    }

    if (pBlobInput->length < targetData.offset + targetData.length) {
        printE("invalid buffer area");
        return FALSE;
    }

    *pTempUint8 = *(pBlobInput->data + targetData.offset);

    return TRUE;
}


//In    pBlobInput
//In    targetData
//Out   outputParam
//return    boolean
boolean getValue_uint16(const BlobData* pBlobInput, const TlvDecodeData targetData, uint16* pTempUint16)
{
    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pTempUint16 == NULL) {
        printE("output buffer is NULL");
        return FALSE;
    }

    if (pBlobInput->length < targetData.offset + targetData.length) {
        printE("invalid buffer area");
        return FALSE;
    }

    *pTempUint16 = *((uint16*)(pBlobInput->data + targetData.offset));
    return TRUE;
}


//In    pBlobInput
//In    targetData
//Out   outputParam
//return    boolean
boolean getValue_uint32(const BlobData* pBlobInput, const TlvDecodeData targetData, uint32* pTempUint32)
{
    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pTempUint32 == NULL) {
        printE("output buffer is NULL");
        return FALSE;
    }

    if (pBlobInput->length < targetData.offset + targetData.length) {
        printE("invalid buffer area");
        return FALSE;
    }

    *pTempUint32 = *((uint32*)(pBlobInput->data + targetData.offset));
    return TRUE;
}

#if 0
uint8 char2Uint8(uint8 input)
{
    printD("input:%x, 'a':%x, 'z':%x", input, 'a', 'z');
    if(input >= '0' && input <= '9') {
        input -= '0';
    }
#if 0
    else if(input >= 'a' && input <= 'f') {
        input -= 'a' - 10;
        printE("getValue(): 2");
    }
#endif
#if 0
    else {
        input = 0xFF;
        printE("else");
    }
#endif
    printD("result input:%x", input);

    return input;
}
#endif

uint32 str2Uint32(const uint8* pInput)
{
#if 0
    uint32 result = 0;

    result = *(pInput + 3) & 0xFF;  //1byte
    result <<= 8; //8bit
    result |= *(pInput + 2) & 0xFF;
    result <<= 8; //8bit
    result |= *(pInput + 1) & 0xFF;
    result <<= 8; //8bit
    result |= *pInput & 0xFF;

    return result;
#endif
    return *((uint32*)pInput);
}

uint16 str2Uint16(const uint8* pInput)
{
    return *((uint16*)pInput);
}



//In    tag
//In    length
//In    data
//Out   tlvEncData
boolean setTlv(const uint32 tag, const uint32 length, const void* pData, TlvEncodeData* pTlvEncData)
{
    if(pData == NULL) {
        printE("pData is NULL");
        return FALSE;
    }
    if(pTlvEncData == NULL || pTlvEncData->data == NULL) {
        printE("pTlvEncData or pTlvEncData->pData is NULL");
        return FALSE;
    }

    pTlvEncData->tag = tag;
    pTlvEncData->length = length;
    memcpy(pTlvEncData->data, pData, pTlvEncData->length);

    return TRUE;
}




//In    inputParam
//In    offset: it points start point from tlvData.
//Out   result
//return    offset + length
uint32 decode(const BlobData* pBlobInput, const uint32 offset, TlvDecodeData* pResult)
{
    uint64 uint64Offset = offset;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE + LENGTH_SIZE) || pBlobInput->length > UINT32_MAX) {
        printD("input length is invalid");
        return FALSE;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE + LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }
    
    if(pBlobInput->length < uint64Offset + (TAG_SIZE + LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }
    
    if(pResult == NULL) {
        printD("decode(): pResult is NULL");
        return FALSE;
    }

    pResult->tag = getTag(pBlobInput, offset);
    pResult->length = getLength(pBlobInput, offset);
    pResult->offset = offset + TAG_SIZE + LENGTH_SIZE;

    if(pBlobInput->length < pResult->offset + pResult->length) {
        printD("input length is invalid");
        pResult->tag = 0;
        pResult->length = 0;
        pResult->offset = 0;
        return FALSE;
    }

    return pResult->offset + pResult->length;
}


// decode big-endian data
uint32 decodeBE(const BlobData* pBlobInput, const uint32 offset, TlvDecodeData* pResult)
{
    uint64 uint64Offset = offset;

    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE + LENGTH_SIZE) || pBlobInput->length > UINT32_MAX) {
        printD("input length is invalid");
        return FALSE;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE + LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }

    if(pBlobInput->length < uint64Offset + (TAG_SIZE + LENGTH_SIZE)) {
        printD("input length is invalid");
        return FALSE;
    }

    if(pResult == NULL) {
        printD("decode(): pResult is NULL");
        return FALSE;
    }

    pResult->tag = getTagBE(pBlobInput, offset);
    pResult->length = getLengthBE(pBlobInput, offset);
    pResult->offset = offset + TAG_SIZE + LENGTH_SIZE;

    if(pBlobInput->length < pResult->offset + pResult->length) {
        printD("input length is invalid");
        pResult->tag = 0;
        pResult->length = 0;
        pResult->offset = 0;
        return FALSE;
    }

    return pResult->offset + pResult->length;
}


// decode big-endian data
uint32 decode2ByteBE(const BlobData* pBlobInput, const uint32 offset, TlvDecodeData* pResult)
{
    uint64 uint64Offset = offset;

    printD("decode2ByteBE");
    CHECK_BLOB_POINTER(pBlobInput, FALSE);

    if(pBlobInput->length < (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE) || pBlobInput->length > UINT32_MAX) {
        printD("input length is invalid. %d", pBlobInput->length);
        return FALSE;
    }

    if(uint64Offset > UINT32_MAX - (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE)) {
        printD("input length is invalid. %d", uint64Offset);
        return FALSE;
    }

    if(pBlobInput->length < uint64Offset + (TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE)) {
        printD("input length is invalid");
        return FALSE;
    }

    if(pResult == NULL) {
        printD("decode(): pResult is NULL");
        return FALSE;
    }

    pResult->tag = getTag2ByteBE(pBlobInput, offset);
    pResult->length = getLength2ByteBE(pBlobInput, offset);
    pResult->offset = offset + TAG_SIZE_2BYTE + LENGTH_SIZE_2BYTE;

    if(pBlobInput->length < pResult->offset + pResult->length) {
        printD("input length is invalid");
        pResult->tag = 0;
        pResult->length = 0;
        pResult->offset = 0;
        return FALSE;
    }

    return pResult->offset + pResult->length;
}


//In    input
//Out   tlvData
//Out   tlvDataLen
//encodeTlvEnc2Blob
boolean encodeTlv2Blob(const TlvEncodeData tlvInput, BlobData* pBlobOutput)
{
    if(tlvInput.tag == 0) {
        printE("tlvInput.tag error");
        return FALSE;
    }
    CHECK_BLOB_POINTER(pBlobOutput, FALSE);
    if(pBlobOutput->dataSize < (TAG_SIZE + LENGTH_SIZE + tlvInput.length)) {
        printE("buffer too small");
        return FALSE;
    }

    memset(pBlobOutput->data, 0, pBlobOutput->dataSize);

    pBlobOutput->length = 0;
    memcpy(pBlobOutput->data, &tlvInput.tag, sizeof(tlvInput.tag));
    pBlobOutput->length += sizeof(tlvInput.tag);

    memcpy(pBlobOutput->data + pBlobOutput->length, &tlvInput.length, sizeof(tlvInput.length));
    pBlobOutput->length += sizeof(tlvInput.length);

    if(tlvInput.length != 0) {
        memcpy((pBlobOutput->data + pBlobOutput->length), tlvInput.data, tlvInput.length);
        pBlobOutput->length += tlvInput.length;
    }

    return TRUE;
}

//pData is allowed NULL
boolean encodeRawTlv2Blob(const uint32 tag, const uint32 length, const void* pData, BlobData* pBlobEncData)
{
    TlvEncodeData inputTlv = {0,};
    boolean ret = TRUE;

    CHECK_BLOB_POINTER(pBlobEncData, FALSE);

    if(pBlobEncData->dataSize < (TAG_SIZE + LENGTH_SIZE + length)) {
        printE("buffer too small");
        return FALSE;
    }

    inputTlv.tag = tag;
    inputTlv.length = length;
    inputTlv.data = (void*)pData;
    ret = encodeTlv2Blob(inputTlv, pBlobEncData);
    if(ret == FALSE) {
        printE("encode error.");
        return FALSE;
    }

    return TRUE;
}