////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                                                                                     /
// This library is free software; you can redistribute it and/or modify it under the terms of the  GNU  Lesser  General/
// Public License as published by the Free Software Foundation; either version 3.0 of the License, or (at your  option)/
// any later version.                                                                                                  /
// This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even  the  implied/
// warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public License for more/
// details.                                                                                                            /
// You should have received a copy of the GNU Lesser General Public License along with this library.                   /
//                                                                                                                     /
// 2012-2021 (c) Baical                                                                                                /
//                                                                                                                     /
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "uP7.h"
#include "uP7hash.h"
#include "uP7context.h"
#include "uP7protocol.h"


static struct stuP7Module    g_pModulesStub[]   = {{"Stub", euP7Level_Critical, 0xFFFF, 0}};
static struct stuP7telemetry g_pTelemetryStub[] = {{"Stub", 0, false, 0xFFFF}};
static struct stuP7arg       g_pArgStub[]       = { {euP7_arg_int64, 8}};
static struct stuP7Trace     g_pTracesStub[]    = {{0xFFFF, sizeof(g_pArgStub)/sizeof(struct stuP7arg), g_pArgStub}};


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                         Context & global variables                                                 //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

static struct stuP7context g_puP7 = 
{
    .bInitialized         = false,             /**< initialized flag */
    .uSessionId           = 0,                 /**< Session ID */
    .bSessionIdCrc7       = 0,                 /**< Session ID crc */
    .pCtxTimer            = NULL,              /**< Context for timer functions */
    .cbTimerFrequency     = NULL,              /**< callback to get system high-resolution timer frequency */
    .cbTimerValue         = NULL,              /**< callback to get system high-resolution timer value */
    .pCtxLock             = NULL,              /**< Context for lock functions, set it to NULL if there is no RTOS*/
    .cbLock               = NULL,              /**< callback to call OS lock function, set it to NULL if there is no RTOS */
    .cbUnLock             = NULL,              /**< callback to call OS unlock function, set it to NULL if there is no RTOS */
    .pCtxPacket           = NULL,              /**< Context for FIFO */
    .cbSendPacket         = NULL,              /**< callback to push data to FIFO */  
    .pCtxThread           = NULL,              /**< Context for FIFO */
    .cbGetCurrentThreadId = NULL,              /**< callback get current thread ID, set it to NULL if there is no RTOS*/  
    .eDefaultVerbosity    = euP7Level_Warning, /**< Default verbosity*/  
    .uTrcCounter          = 0,                 /**< Trace items counter*/  
    .pModules             = g_pModulesStub,    /**< Trace modules descriptors, generated by uP7 preprocessor*/  
    .szModules            = 1,                 /**< Trace modules descriptors count*/  
    .pTraces              = g_pTracesStub,     /**< Trace descriptors, generated by uP7 preprocessor*/  
    .szTraces             = 1,                 /**< Trace descriptors count*/  
    .bTraceDirect         = false,             /**< Is it possible to access trace descriptor by ID direcly*/  
    .pTelemetry           = g_pTelemetryStub,  /**< Telemetry descriptors, generated by uP7 preprocessor*/  
    .szTelemetry          = 1,                 /**< Telemetry descriptors count*/  
    .szAlignmentMask      = euP7alignmentNone, /**< transport packet size alignment*/      
    .szAlignment          = 0,                 /**< transport packet size alignment mask*/      
    .szControl            = 0                  /**< Size of control packet head*/  
};

static const enum euP7packetRank g_pRankMap[euP7Levels_Count] =
{
    euP7packetRankLow,      //euP7Level_Trace      
    euP7packetRankLow,      //euP7Level_Debug      
    euP7packetRankMedium,   //euP7Level_Info       
    euP7packetRankMedium,   //euP7Level_Warning    
    euP7packetRankHigh,     //euP7Level_Error      
    euP7packetRankHighest   //euP7Level_Critical   
};


#if defined(UP7_LOCK_AVAILABLE)
    #define uP7_LOCK()   g_puP7.cbLock(g_puP7.pCtxLock);
    #define uP7_UNLOCK() g_puP7.cbUnLock(g_puP7.pCtxLock);
#else
    #define uP7_LOCK()   
    #define uP7_UNLOCK() 
#endif


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//                                                  Code                                                              //
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7initialize(const struct stuP7config *i_pConfig)
{
    if (g_puP7.bInitialized)
    {
        return true;
    }

    if (    (!i_pConfig)
         || (!i_pConfig->cbTimerFrequency)
         || (!i_pConfig->cbTimerValue)
         || (!i_pConfig->cbSendPacket)
       )
    {
        return false;
    }

    g_puP7.bSessionIdCrc7       = i_pConfig->bSessionIdCrc7;
    g_puP7.pCtxTimer            = i_pConfig->pCtxTimer; 
    g_puP7.cbTimerFrequency     = i_pConfig->cbTimerFrequency;           
    g_puP7.cbTimerValue         = i_pConfig->cbTimerValue;           
    g_puP7.pCtxLock             = i_pConfig->pCtxLock;
    g_puP7.cbLock               = i_pConfig->cbLock;
    g_puP7.cbUnLock             = i_pConfig->cbUnLock;
    g_puP7.pCtxPacket           = i_pConfig->pCtxPacket;
    g_puP7.cbSendPacket         = i_pConfig->cbSendPacket;
    g_puP7.pCtxThread           = i_pConfig->pCtxThread;
    g_puP7.cbGetCurrentThreadId = i_pConfig->cbGetCurrentThreadId;
    g_puP7.eDefaultVerbosity    = i_pConfig->eDefaultVerbosity;
    g_puP7.qwCreationTime       = g_puP7.cbTimerValue(g_puP7.pCtxTimer);
    g_puP7.szTelChunks          = 1;
    g_puP7.uSessionId           = i_pConfig->uSessionId;
    g_puP7.szAlignmentMask      = i_pConfig->eAlignment;
    g_puP7.szAlignment          = g_puP7.szAlignmentMask + 1;
    g_puP7.szControl            = 0;

    memset(g_puP7.pTail, 0, UP7_TAIL);
               
    if (    (i_pConfig->pModules)
         && (i_pConfig->szModules)
       )
    {
        g_puP7.pModules  = i_pConfig->pModules;   
        g_puP7.szModules = i_pConfig->szModules;  
    }

    if (    (i_pConfig->pTraces)
         && (i_pConfig->szTraces)
       )
    {
        g_puP7.pTraces  = i_pConfig->pTraces;    
        g_puP7.szTraces = i_pConfig->szTraces;   
    }

    if (    (i_pConfig->pTelemetry)
         && (i_pConfig->szTelemetry)
       )
    {
        g_puP7.pTelemetry  = i_pConfig->pTelemetry; 
        g_puP7.szTelemetry = i_pConfig->szTelemetry;
    }


    //check string types deducted by preprocessor and run-time
    for (size_t l_szI = 0; l_szI < g_puP7.szTraces; l_szI++)
    {
        for (uint16_t l_wJ = 0; l_wJ < g_puP7.pTraces[l_szI].wArgsCount; l_wJ++)
        {
            if (    (euP7_arg_str_utf16 == g_puP7.pTraces[l_szI].pArgs[l_wJ].eType)
                 || (euP7_arg_char16 == g_puP7.pTraces[l_szI].pArgs[l_wJ].eType)
               )
            {
                if (4 == sizeof(wchar_t))
                {
                    return false;
                }
            }
            else if (    (euP7_arg_str_utf32 == g_puP7.pTraces[l_szI].pArgs[l_wJ].eType)
                      || (euP7_arg_char32 == g_puP7.pTraces[l_szI].pArgs[l_wJ].eType)
                    )
            {
                if (2 == sizeof(wchar_t))
                {
                    return false;
                }
            }
        }
    }


#if defined(UP7_LOCK_AVAILABLE)
    if (    (!i_pConfig->cbLock)
         || (!i_pConfig->cbUnLock)
       )
    {
        return false;
    }

#endif

    g_puP7.bInitialized = true;

    g_puP7.stTelChunk[0].pData  = &g_puP7.stTelSample;
    g_puP7.stTelChunk[0].szData = sizeof(g_puP7.stTelSample);

    g_puP7.stTelChunk[1].pData  = g_puP7.pTail;
    g_puP7.stTelChunk[1].szData = g_puP7.szAlignment - (sizeof(g_puP7.stTelSample) & g_puP7.szAlignmentMask);

#if   UP7_PRECISE_TYPE_INDEX == 0
    g_puP7.stTelSample.stBase.bType = (uint8_t)uP7packetTelemetryDouble;
#elif UP7_PRECISE_TYPE_INDEX == 1
    g_puP7.stTelSample.stBase.bType = (uint8_t)uP7packetTelemetryFloat;
#elif UP7_PRECISE_TYPE_INDEX == 2
    g_puP7.stTelSample.stBase.bType = (uint8_t)uP7packetTelemetryInt64;
#elif UP7_PRECISE_TYPE_INDEX == 3
    g_puP7.stTelSample.stBase.bType = (uint8_t)uP7packetTelemetryInt32;
#else
    char assert_unknown_telemetry_type[-1];
#endif
    g_puP7.stTelSample.stBase.uSessionId = g_puP7.uSessionId;
    g_puP7.stTelSample.stBase.bExt1Crc7  = g_puP7.bSessionIdCrc7;
    g_puP7.stTelSample.stBase.wSize      = sizeof(stuP7telHdr);

    if (g_puP7.stTelChunk[1].szData != g_puP7.szAlignment)
    {
        g_puP7.stTelSample.stBase.wSize += (uint16_t)g_puP7.stTelChunk[1].szData;
        g_puP7.szTelChunks ++;
    }


    ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //create traces descriptors tree
    g_puP7.bTraceDirect = true;
    for (size_t l_szI = 0; l_szI < g_puP7.szTraces; l_szI++)
    {
        if (g_puP7.pTraces[l_szI].wId != l_szI)
        {
            g_puP7.bTraceDirect = false;
            break;
        }
    }

    return g_puP7.bInitialized;
}



////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void uP7unInitialize()
{
    if (!g_puP7.bInitialized)
    {
        return;
    }

    g_puP7.bInitialized = false;
    memset(&g_puP7, 0, sizeof(g_puP7));
}



ST_ASSERT(sizeof(struct stuP7traceVerbosityHdr) <= UP7_CONTROL);
ST_ASSERT(sizeof(struct stuP7telOnOffHdr) <= UP7_CONTROL);
ST_ASSERT(sizeof(struct stuP7timeReqHdr) <= UP7_CONTROL);

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
size_t uP7process(const uint8_t *i_pData, size_t i_szData)
{
    if (    (!g_puP7.bInitialized)
         || (!i_pData)
       )
    {
        return 0;
    }

    uP7_LOCK();
    struct stuP7baseHdr *l_pBase    = (struct stuP7baseHdr *)g_puP7.pControl;
    size_t               l_szReturn = i_szData;
    size_t               l_szCopy   = 0;

    while ((i_szData + g_puP7.szControl) >= sizeof(struct stuP7baseHdr))
    {
        if (g_puP7.szControl < sizeof(struct stuP7baseHdr))
        {
            l_szCopy = sizeof(struct stuP7baseHdr) - g_puP7.szControl;
            memcpy(g_puP7.pControl + g_puP7.szControl, i_pData, l_szCopy);
            g_puP7.szControl  = sizeof(struct stuP7baseHdr);
            i_szData         -= l_szCopy;
            i_pData          += l_szCopy;
        }

        l_szCopy = l_pBase->wSize - g_puP7.szControl;

        if (l_szCopy <= i_szData)
        {
            if (    (l_pBase->uSessionId != g_puP7.uSessionId)
                 || ((l_pBase->bExt1Crc7 & 0x7F) != g_puP7.bSessionIdCrc7)
               )
            {
                //if session ID or CRC do not match - move out first byte and going for new cycle -> data is corrupted
                --g_puP7.szControl;
                memmove(g_puP7.pControl, g_puP7.pControl + 1, g_puP7.szControl);
                continue;
            }
            else if (l_szCopy)
            {
                memcpy(g_puP7.pControl + g_puP7.szControl, i_pData, l_szCopy);
            }

            if (    (uP7packetTraceVerbosiry == l_pBase->bType)
                 && (l_pBase->wSize == sizeof(struct stuP7traceVerbosityHdr))
               )
            {
                struct stuP7traceVerbosityHdr *l_pVerb = (struct stuP7traceVerbosityHdr *)g_puP7.pControl;

                if (l_pVerb->wModuleId < g_puP7.szModules)
                {
                    g_puP7.pModules[l_pVerb->wModuleId].eLevel = l_pVerb->bLevel;
                }
            }
            else if (    (uP7packetTelemetryOnOff == l_pBase->bType)
                      && (l_pBase->wSize == sizeof(struct stuP7telOnOffHdr))
                    )
            {
                struct stuP7telOnOffHdr *l_pOnOff = (struct stuP7telOnOffHdr *)g_puP7.pControl;

                if (l_pOnOff->wId < g_puP7.szTelemetry)
                {
                    g_puP7.pTelemetry[l_pOnOff->wId].bOn = l_pOnOff->bOn;
                }
            }
            else if (    (uP7packetCreateTimeRequest == l_pBase->bType)
                      && (l_pBase->wSize == sizeof(struct stuP7timeReqHdr))
                    )
            {
                //struct stuP7timeReqHdr *l_pTimeReq = (struct stuP7timeReqHdr *)g_puP7.pControl;
                
                struct stuP7timeResHdr l_stTime = { { .uSessionId = g_puP7.uSessionId,
                                                      .wSize      = sizeof(struct stuP7timeResHdr),
                                                      .bType      = uP7packetCreateTimeResponse,
                                                      .bExt1Crc7  = g_puP7.bSessionIdCrc7
                                                    },
                                                    .qwCpuFreq       = g_puP7.cbTimerFrequency(g_puP7.pCtxTimer),
                                                    .qwCpuStartTime  = g_puP7.qwCreationTime,
                                                    .qwCpuCurrenTime = g_puP7.cbTimerValue(g_puP7.pCtxTimer),
                                                  };
                struct stP7packetChunk l_stChunk[2] = { { .pData = &l_stTime,  
                                                          .szData = sizeof(l_stTime)  
                                                        },
                                                        { .pData = g_puP7.pTail, 
                                                          .szData = g_puP7.szAlignment - (sizeof(l_stTime) & g_puP7.szAlignmentMask)
                                                        }
                                                      };

                
                if (g_puP7.szAlignment > l_stChunk[1].szData)
                {
                    l_stTime.stBase.wSize += (uint16_t)l_stChunk[1].szData; 
                    g_puP7.cbSendPacket(g_puP7.pCtxPacket, euP7packetRankHighest, l_stChunk, 2, l_stTime.stBase.wSize);
                }
                else
                {
                    g_puP7.cbSendPacket(g_puP7.pCtxPacket, euP7packetRankHighest, l_stChunk, 1, l_stTime.stBase.wSize);
                }
            }


            i_szData -= l_szCopy;
            i_pData  += l_szCopy;
            g_puP7.szControl = 0;
        }
        else 
        {
            break;
        }
    }

    if (i_szData)
    {
        if ((UP7_CONTROL - g_puP7.szControl) >= i_szData)
        {
            memcpy(g_puP7.pControl + g_puP7.szControl, i_pData, i_szData);
            g_puP7.szControl  += i_szData;
        }
        else
        {
            g_puP7.szControl = 0;
        }

        i_szData = 0;
    }

    uP7_UNLOCK();

    return l_szReturn;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void uP7TrcSetVerbosity(huP7Module i_hModule, enum euP7Level i_eVerbosity)
{
    if (    (!g_puP7.bInitialized)
         || (i_hModule >= g_puP7.szModules)
       )
    {
        return;
    }

    uP7_LOCK();

    struct stuP7Module *l_pMod = &g_puP7.pModules[i_hModule];

    l_pMod->eLevel = i_eVerbosity;

    struct stuP7traceVerbosityHdr l_stVerb = { {.uSessionId = g_puP7.uSessionId,
                                                .wSize      = sizeof(struct stuP7traceVerbosityHdr),
                                                .bType      = uP7packetTraceVerbosiry,
                                                .bExt1Crc7  = g_puP7.bSessionIdCrc7
                                               },
                                               .wModuleId = l_pMod->wId,
                                               .bLevel  = (uint8_t)i_eVerbosity
                                             };


    struct stP7packetChunk l_stChunk[2] = {   { .pData = &l_stVerb,
                                                .szData = sizeof(l_stVerb)      
                                              },
                                              { .pData = g_puP7.pTail, 
                                                .szData = g_puP7.szAlignment - (sizeof(l_stVerb) & g_puP7.szAlignmentMask)
                                              }
                                          };

            
    if (g_puP7.szAlignment > l_stChunk[1].szData)
    {
        l_stVerb.stBase.wSize += (uint16_t)l_stChunk[1].szData;
        g_puP7.cbSendPacket(g_puP7.pCtxPacket, euP7packetRankHighest, l_stChunk, 2, l_stVerb.stBase.wSize);
    }
    else
    {
        g_puP7.cbSendPacket(g_puP7.pCtxPacket, euP7packetRankHighest, l_stChunk, 1, l_stVerb.stBase.wSize);
    }

    uP7_UNLOCK();
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TrcRegisterModule(const char *i_pName, enum euP7Level i_eVerbosity, huP7Module *o_hModule)
{
    if (    (!g_puP7.bInitialized)
         || (!o_hModule)
         || (!i_pName)
       )
    {
        return false;
    }
    
    uP7_LOCK();
    
    uint32_t l_uHash = _getHash(i_pName);

    *o_hModule = uP7_MODULE_INVALID_ID;

    for (size_t l_szI = 0; l_szI < g_puP7.szModules; l_szI++)
    {
        if (    (g_puP7.pModules[l_szI].uHash == l_uHash)
             && (0 == strcmp(i_pName, g_puP7.pModules[l_szI].pName))
           )
        {
            g_puP7.pModules[l_szI].eLevel = i_eVerbosity;
            *o_hModule = g_puP7.pModules[l_szI].wId;
            break;
        }
    }
    
    uP7_UNLOCK();

    return (*o_hModule != uP7_MODULE_INVALID_ID) ? true : false;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TrcFindModule(const char *i_pName, huP7Module *o_hModule)
{
    if (    (!g_puP7.bInitialized)
         || (!o_hModule)
         || (!i_pName)
       )
    {
        return false;
    }
    
    uP7_LOCK();
    
    uint32_t l_uHash = _getHash(i_pName);

    *o_hModule = uP7_MODULE_INVALID_ID;

    for (size_t l_szI = 0; l_szI < g_puP7.szModules; l_szI++)
    {
        if (    (g_puP7.pModules[l_szI].uHash == l_uHash)
             && (0 == strcmp(i_pName, g_puP7.pModules[l_szI].pName))
           )
        {
            *o_hModule = g_puP7.pModules[l_szI].wId;
            break;
        }
    }
    
    uP7_UNLOCK();

    return (*o_hModule != uP7_MODULE_INVALID_ID) ? true : false;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TrcSent(uint16_t       i_wId,
                enum euP7Level i_eLevel, 
                huP7Module     i_hModule,
                ...)
{
    bool    l_bReturn;
    va_list l_pVl;

    va_start(l_pVl, i_hModule);
    l_bReturn = uP7TrcSentEmb(i_wId, i_eLevel, i_hModule, &l_pVl);
    va_end(l_pVl);

    return l_bReturn;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TrcSentEmb(uint16_t       i_wId,
                   enum euP7Level i_eLevel, 
                   huP7Module     i_hModule,
                   va_list       *i_pVa_List)
{
    struct stP7packetChunk  l_pChunks[uP7_MAX_CHUNKS];
    uint8_t                 l_pArgsData[uP7_MAX_TRACE_ARGS];
    bool                    l_bReturn     = true;
    uint16_t                l_wArgsFree   = uP7_MAX_TRACE_ARGS; 
    size_t                  l_szChunks    = 0;               
    struct stP7packetChunk *l_pChunksIt   = l_pChunks;
    uint8_t                *l_pArgsDataIt = l_pArgsData;
    size_t                  l_szData      = 0;
    size_t                  l_szTail;    //not initialized on purpose
    enum euP7packetRank     l_eRank;     //not initialized on purpose
    struct stuP7traceHdr    l_stTrace;   //not initialized on purpose
    struct stuP7Trace      *l_pTraceDesc;//not initialized on purpose
    const struct stuP7arg  *l_pArgs;     //not initialized on purpose
    const struct stuP7arg  *l_pArgsEnd;  //not initialized on purpose 

    uP7_LOCK();
    if (    (!g_puP7.bInitialized)
         || ((i_hModule < g_puP7.szModules)  && (i_eLevel < g_puP7.pModules[i_hModule].eLevel))
         || ((i_hModule >= g_puP7.szModules) && (i_eLevel < g_puP7.eDefaultVerbosity))
       )
    {
        l_bReturn = false;
        goto l_lblExit;
    }

    g_puP7.uTrcCounter ++;

    if (    (g_puP7.bTraceDirect)
         && (i_wId < g_puP7.szTraces)
       )
    {
        l_pTraceDesc = &g_puP7.pTraces[i_wId];
    }
    else
    {
        l_pTraceDesc = NULL;
        size_t l_szHead = 0;
        size_t l_szTail = g_puP7.szTraces;
        size_t l_szIdx;

        while (l_szTail >= l_szHead) //classical binary search in sorted array
        {
            l_szIdx = l_szHead + ((l_szTail - l_szHead) / 2);
            if (g_puP7.pTraces[l_szIdx].wId == i_wId)
            {
                l_pTraceDesc = &g_puP7.pTraces[l_szIdx];
                break;
            }
            else if (g_puP7.pTraces[l_szIdx].wId > i_wId)
            {
                l_szTail = l_szIdx - 1;
            }
            else
            {
                l_szHead = l_szIdx + 1;
            }
        }
    }


    if (!l_pTraceDesc)
    {
        l_bReturn = false;
        goto l_lblExit;
    }

    l_eRank = g_pRankMap[i_eLevel];

    l_stTrace.stBase.bExt1Crc7  = g_puP7.bSessionIdCrc7;
    l_stTrace.stBase.bType      = (uint8_t)uP7packetTrace;
    l_stTrace.stBase.uSessionId = g_puP7.uSessionId;

    if (i_hModule < g_puP7.szModules)
    {
        l_stTrace.bMod13Lvl3 = up7_SET_TRC_MOD_LVL(g_puP7.pModules[i_hModule].wId, i_eLevel);
    }
    else
    {
        l_stTrace.bMod13Lvl3 = up7_SET_TRC_MOD_LVL(0, i_eLevel);
    }

    l_stTrace.qwTime     = g_puP7.cbTimerValue(g_puP7.pCtxTimer);
    l_stTrace.uSeqN      = g_puP7.uTrcCounter;
    l_stTrace.uThreadId  = g_puP7.cbGetCurrentThreadId ? g_puP7.cbGetCurrentThreadId(g_puP7.pCtxThread) : 0;
    l_stTrace.wId        = i_wId;

    l_pChunksIt->pData   = &l_stTrace;
    l_pChunksIt->szData  = sizeof(l_stTrace);
    l_szData += l_pChunksIt->szData;
    l_pChunksIt++;
    l_szChunks++;

    l_pChunksIt->pData  = l_pArgsDataIt;
    l_pChunksIt->szData = 0;

    l_pArgs     = l_pTraceDesc->pArgs;
    l_pArgsEnd  = l_pArgs + l_pTraceDesc->wArgsCount; 

    while (    (l_pArgs < l_pArgsEnd)
            && (l_wArgsFree > 8) 
            && (l_szChunks < uP7_MAX_CHUNKS) 
          )
    {
        if (4 == l_pArgs->eStackSize) //most probable case
        {
            *(uint32_t*)l_pArgsDataIt = va_arg(*i_pVa_List, uint32_t);    

            l_pChunksIt->szData += 4;//l_pArgs->eStackSize;
            l_pArgsDataIt       += 4;//l_pArgs->eStackSize;
            l_wArgsFree         -= 4;//l_pArgs->eStackSize;
        }
        else if (8 == l_pArgs->eStackSize) //less probable case
        {
        #if defined(UP7_FLOATING_POINT)
            if (euP7_arg_double != l_pArgs->eType)
        #endif
            {
                uint64_t  l_qwValue = va_arg(*i_pVa_List, uint64_t);
                memcpy(l_pArgsDataIt, &l_qwValue, sizeof(l_qwValue));
                //((uint32_t*)l_pArgsDataIt)[0] = ((uint32_t*)&l_qwValue)[0];
                //((uint32_t*)l_pArgsDataIt)[1] = ((uint32_t*)&l_qwValue)[1];
            }
        #if defined(UP7_FLOATING_POINT)
            else
            {
                double l_dbValue = va_arg(*i_pVa_List, double);
                memcpy(l_pArgsDataIt, &l_dbValue, sizeof(l_dbValue));
                //((uint32_t*)l_pArgsDataIt)[0] = ((uint32_t*)&l_dbValue)[0];
                //((uint32_t*)l_pArgsDataIt)[1] = ((uint32_t*)&l_dbValue)[1];
            }
        #endif
            l_pChunksIt->szData += 8;//l_pArgs->eStackSize;
            l_pArgsDataIt       += 8;//l_pArgs->eStackSize;
            l_wArgsFree         -= 8;//l_pArgs->eStackSize;
        }
        else //string 
        {
            if (l_pChunksIt->szData) //if current block has data - move to empty one
            {
                l_szData += l_pChunksIt->szData;
                l_pChunksIt++;
                l_szChunks++;
            }

            if (    (euP7_arg_str_utf8 == l_pArgs->eType) 
                 || (euP7_arg_str_ansi == l_pArgs->eType) 
               )
            {
                l_pChunksIt->pData = va_arg(*i_pVa_List, char*);

                if (uP7_STRING_SUPPORTED_SIZE == l_pArgs->eStackSize)
                {
                    if (l_pChunksIt->pData)
                    {
                        l_pChunksIt->szData = strlen((char*)l_pChunksIt->pData) + 1;
                    }
                    else
                    {
                        l_pChunksIt->pData = "(NULL)";
                        l_pChunksIt->szData = 7;
                    }
                }
                else
                {
                    static uint8_t g_pUnsupported8[] = {'(', '[', 'x', ']', 's', ' ', 'i', 's', ' ', 'u', 'n', 's', 'u', 
                                                        'p', 'p', 'o', 'r', 't', 'e', 'd', ' ', 'b', 'y', ' ', 'u', 'P',
                                                        '7', ')', 0};
                    static uint8_t g_pPadding8[uP7_STRING_UNSUPPORTED_SIZE]; (void)g_pPadding8;
                    l_pChunksIt->pData  = g_pUnsupported8;
                    l_pChunksIt->szData = sizeof(g_pUnsupported8) + uP7_STRING_UNSUPPORTED_SIZE;
                }
            }
            else if (euP7_arg_str_utf32 == l_pArgs->eType) 
            {
                l_pChunksIt->pData = va_arg(*i_pVa_List, uint32_t*);

                if (uP7_STRING_SUPPORTED_SIZE == l_pArgs->eStackSize)
                {
                    if (l_pChunksIt->pData)
                    {
                        uint32_t *l_pStrTail = l_pChunksIt->pData;
                        while (*l_pStrTail++); //looking for the end
                        l_pStrTail++; //0 terminator

                        l_pChunksIt->szData = sizeof(uint32_t) * (l_pStrTail - (uint32_t*)l_pChunksIt->pData);
                    }
                    else
                    {
                        static uint32_t g_pNull32[] = {'(', 'N', 'U', 'L', 'L', ')', 0};
                        l_pChunksIt->pData = g_pNull32;
                        l_pChunksIt->szData = sizeof(g_pNull32);
                    }
                }
                else
                {
                    static uint32_t g_pUnsupported32[] = {'(', '[', 'x', ']', 's', ' ', 'i', 's', ' ', 'u', 'n', 's', 'u', 
                                                          'p', 'p', 'o', 'r', 't', 'e', 'd', ' ', 'b', 'y', ' ', 'u', 'P',
                                                          '7', ')', 0};
                    static uint8_t g_pPadding32[uP7_STRING_UNSUPPORTED_SIZE]; (void)g_pPadding32;
                    l_pChunksIt->pData  = g_pUnsupported32;
                    l_pChunksIt->szData = sizeof(g_pUnsupported32) + uP7_STRING_UNSUPPORTED_SIZE;
                }
            }
            else if (euP7_arg_str_utf16 == l_pArgs->eType) 
            {
                l_pChunksIt->pData = va_arg(*i_pVa_List, uint16_t*);

                if (uP7_STRING_SUPPORTED_SIZE == l_pArgs->eStackSize)
                {
                    if (l_pChunksIt->pData)
                    {
                        uint16_t *l_pStrTail = l_pChunksIt->pData;
                        while (*l_pStrTail++); //looking for the end
                        l_pStrTail++; //0 terminator

                        l_pChunksIt->szData = sizeof(uint16_t) * (l_pStrTail - (uint16_t*)l_pChunksIt->pData);
                    }
                    else
                    {
                        static uint16_t g_pNull16[] = {'(', 'N', 'U', 'L', 'L', ')', 0};
                        l_pChunksIt->pData = g_pNull16;
                        l_pChunksIt->szData = sizeof(g_pNull16);
                    }
                }
                else
                {
                    static uint16_t g_pUnsupported16[] = {'(', '[', 'x', ']', 's', ' ', 'i', 's', ' ', 'u', 'n', 's', 'u', 
                                                          'p', 'p', 'o', 'r', 't', 'e', 'd', ' ', 'b', 'y', ' ', 'u', 'P',
                                                          '7', ')', 0};
                    static uint8_t g_pPadding16[uP7_STRING_UNSUPPORTED_SIZE]; (void)g_pPadding16;
                    l_pChunksIt->pData  = g_pUnsupported16;
                    l_pChunksIt->szData = sizeof(g_pUnsupported16) + uP7_STRING_UNSUPPORTED_SIZE;
                }
            }

            l_szData += l_pChunksIt->szData;
            l_pChunksIt++;
            l_szChunks++;

            l_pChunksIt->pData  = l_pArgsDataIt;
            l_pChunksIt->szData = 0;
        }
      


        l_pArgs ++;
    }

    if (    (l_pChunksIt)
         && (l_pChunksIt->szData)
       )
    {
        l_szChunks ++;
        l_szData  += l_pChunksIt->szData;
        l_pChunksIt ++;
    }

    l_szTail = l_szData & g_puP7.szAlignmentMask;
    if (l_szTail)
    {
        l_pChunksIt->pData  = g_puP7.pTail; //tail contains 0 to avoid interpretation as extension
        l_pChunksIt->szData = g_puP7.szAlignment - l_szTail;

        l_szData += l_pChunksIt->szData;
        l_szChunks ++;
    }


    if (l_pArgs != l_pArgsEnd) //not all arguments were processed
    {
        l_bReturn = false;
        goto l_lblExit;
    }

    l_stTrace.stBase.wSize = (uint16_t)l_szData;

    if (!g_puP7.cbSendPacket(g_puP7.pCtxPacket, l_eRank, l_pChunks, l_szChunks, l_szData))
    {
        l_bReturn = false;
        goto l_lblExit;
    }

l_lblExit:
    uP7_UNLOCK(); 

    return l_bReturn;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TelCreateCounter(const char *i_pName, 
                         tuP7TelVal  i_tMin,
                         tuP7TelVal  i_tbAlarmMin,
                         tuP7TelVal  i_tMax,
                         tuP7TelVal  i_tAlarmMax,
                         bool        i_bOn,
                         huP7TelId  *o_pID 
                        )
{
    UNUSED_ARG(i_tMin);
    UNUSED_ARG(i_tbAlarmMin);
    UNUSED_ARG(i_tMax);
    UNUSED_ARG(i_tAlarmMax);

    if (uP7TelFindCounter(i_pName, o_pID))
    {
        uP7_LOCK();
        g_puP7.pTelemetry[*o_pID].bOn = i_bOn;
        uP7_UNLOCK();
        return true;
    }

    return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TelSentSample(huP7TelId i_hID, tuP7TelVal i_tValue)
{
    uP7_LOCK();
    if (    (!g_puP7.bInitialized)
         || (i_hID >= g_puP7.szTelemetry)
         || (!g_puP7.pTelemetry[i_hID].bOn)
       )
    {
        uP7_UNLOCK(); 
        return false;
    }

    g_puP7.stTelSample.wId    = i_hID;
    g_puP7.stTelSample.tValue = i_tValue;
    g_puP7.stTelSample.qwTime = g_puP7.cbTimerValue(g_puP7.pCtxTimer);

    if (!g_puP7.cbSendPacket(g_puP7.pCtxPacket, 
                             euP7packetRankLow, 
                             g_puP7.stTelChunk, 
                             g_puP7.szTelChunks, 
                             g_puP7.stTelSample.stBase.wSize
                            )
       )
    {
        uP7_UNLOCK(); 
        return false;
    }

    uP7_UNLOCK();

    return true;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TelFindCounter(const char *i_pName, huP7TelId *o_pID)
{
    if (    (!g_puP7.bInitialized)
         || (!o_pID)
         || (!i_pName)
       )
    {
        return false;
    }
    
    uP7_LOCK();
    
    uint32_t l_uHash = _getHash(i_pName);

    *o_pID = uP7_TELEMETRY_INVALID_ID;

    for (size_t l_szI = 0; l_szI < g_puP7.szTelemetry; l_szI++)
    {
        if (    (g_puP7.pTelemetry[l_szI].uHash == l_uHash)
             && (0 == strcmp(i_pName, g_puP7.pTelemetry[l_szI].pName))
           )
        {
            *o_pID = g_puP7.pTelemetry[l_szI].wId;
            break;
        }
    }
    
    uP7_UNLOCK();

    return (uP7_TELEMETRY_INVALID_ID != *o_pID) ? true : false;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void uP7TelEnableCounter(huP7TelId i_hID, bool i_bEnable)
{
    if (i_hID < g_puP7.szTelemetry)
    {
        uP7_LOCK();
        g_puP7.pTelemetry[i_hID].bOn = i_bEnable;
        uP7_UNLOCK();
    }
}
    

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool uP7TelIsCounterEnabled(huP7TelId i_hID)
{
    uP7_LOCK();
    bool l_bReturn = (i_hID < g_puP7.szTelemetry) ? g_puP7.pTelemetry[i_hID].bOn : false;
    uP7_UNLOCK();

    return l_bReturn;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
const char *uP7TelGetCounterName(huP7TelId i_hID)
{
    uP7_LOCK();
    const char *l_pReturn = (i_hID < g_puP7.szTelemetry) ? g_puP7.pTelemetry[i_hID].pName : NULL;
    uP7_UNLOCK();

    return l_pReturn;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
size_t uP7TelGetCountersAmount()
{
    return g_puP7.szTelemetry;
}