/*
 * Copyright (c) 2025, Texas Instruments Incorporated
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * *  Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * *  Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * *  Neither the name of Texas Instruments Incorporated nor the names of
 *    its contributors may be used to endorse or promote products derived
 *    from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
 * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include <ti/ai/edge_ai/dap/DAP.h>
#include <ti/ai/edge_ai/dap/core/DAP_core.h>
#include <ti/ai/edge_ai/dap/interface/DAP_interface.h>

#include <ti/drivers/UART2.h>
#include <ti/drivers/dpl/HwiP.h>
#include <ti/drivers/dpl/MessageQueueP.h>

#define DAP_API_VERSION 1
/* Capabilities of the DAP:
 * 0 - not supported
 * 1 - supported
 */
#define CRC_CHECKSUM    0
#define BIDIRECTIONAL   0
#define BIG_ENDIAN      1
#define MODEL_UPLOAD    0
#define INFERENCE       1

#define CAPABILITIES_FLAG \
    ((INFERENCE) | (MODEL_UPLOAD << 1) | (BIG_ENDIAN << 2) | (BIDIRECTIONAL << 3) | (CRC_CHECKSUM << 4))

/* Not yet implemented in host side */
#define ERROR_NUMBER 0

/* FRAME STRUCTURE */
/* SOF */
#define START_IDX        (0)
#define START_BYTE_SIZE  (1)
#define FRAME_START_BYTE (0xED)

/* Command Byte */
#define CMD_IDX  (START_IDX + START_BYTE_SIZE)
#define CMD_SIZE (1)

/* Length Byte */
#define LEN_IDX (CMD_IDX + CMD_SIZE)

/* EOF */
#define END_OFFSET     (1)
#define FRAME_END_BYTE (0x9E)

static void DAP_getCapabilities(DAP_Handle *dapHandle);
static void DAP_listAllSensors(DAP_Handle *dapHandle);
static void DAP_listSensor(DAP_Handle *dapHandle, uint8_t sensorCount, uint8_t sensorIndex);
static void DAP_configurePipeline(DAP_Handle *dapHandle, DAP_Frame *frame);
static void DAP_listAllModels(DAP_Handle *dapHandle);
static void DAP_listModel(DAP_Handle *dapHandle, uint8_t modelCount, uint8_t modelIndex);
static void DAP_startStreaming(DAP_Handle *dapHandle);
static void DAP_stopStreaming(DAP_Handle *dapHandle);
static void DAP_listAllInferences(DAP_Handle *dapHandle);
static void DAP_listInference(DAP_Handle *dapHandle, uint8_t inferenceCount, uint8_t inferenceIndex);
static void DAP_readProperty(DAP_Handle *dapHandle, uint8_t index);
static void DAP_writeProperty(DAP_Handle *dapHandle, DAP_Frame *frame);
static void DAP_listAllPropeties(DAP_Handle *dapHandle);
static void DAP_listProperty(DAP_Handle *dapHandle, uint8_t propertyCount, uint8_t propertyIndex);
static void DAP_sendDataCommand(DAP_Handle *dapHandle, DAP_Frame *frame);

static void DAP_prepareTxHeader(DAP_Frame *frame, DAP_CommandType command);
static void DAP_prepareTxTrailer(DAP_Frame *frame, bool isCrc);
static void DAP_prepareLengthByte(DAP_Frame *frame, size_t payloadLength);
static void DAP_preparePayload(DAP_Frame *frame, const void *data, size_t dataLength);

DAP_ErrorType DAP_verifyFrame(DAP_Frame *frame)
{
    /* Verify start byte */
    // if (frame->buffer[START_IDX] != FRAME_START_BYTE)
    // {
    //     return DAP_ERROR_TYPE_MISMATCH_START_BYTE;
    // }

    /* Verify end byte */
    if (frame->buffer[frame->length - END_OFFSET] != FRAME_END_BYTE)
    {
        return DAP_ERROR_TYPE_MISMATCH_END_BYTE;
    }

    // /* Verify CRC16 */
    // if(UART_handle->isCrc)
    // {
    //     uint8_t size = HEADER_SIZE + MEM_ADDR_SIZE + UART_handle->dataLen;
    //     if((UART_handle->rxMsg.buffer[CTRL_IDX] & CMD_MASK) == READ_CMD )
    //     {
    //         size = HEADER_SIZE + MEM_ADDR_SIZE + 0;
    //     }
    //     uint16_t checkSum = CRC_calc16((&UART_handle->rxMsg.buffer[START_IDX]),size);
    //     uint16_t expectedCheckSum
    //     = u16(&UART_handle->rxMsg.buffer[UART_handle->rxMsg.len - CRC_OFFSET]);

    //     if(checkSum != expectedCheckSum)
    //     {
    //         UART_handle->error = DAP_ERROR_TYPE_MISMATCH_CRC;
    //         return ;
    //     }
    // }

    return DAP_ERROR_TYPE_NONE;
}

void DAP_serviceCommand(DAP_Handle *dapHandle, DAP_Frame *commandFrame)
{
    uint8_t index;
    DAP_CommandType command = (DAP_CommandType)commandFrame->buffer[CMD_IDX];

    if (command < DAP_SEND_DATA_COMMAND)
    {
        switch (command)
        {
            case DAP_GET_CAPABILITIES_COMMAND:
                DAP_getCapabilities(dapHandle);
                break;

            case DAP_LIST_SENSORS_COMMAND:
                DAP_listAllSensors(dapHandle);
                break;

            case DAP_CONFIGURE_PIPELINE_COMMAND:
                DAP_configurePipeline(dapHandle, commandFrame);
                break;

            case DAP_LIST_MODELS_COMMAND:
                DAP_listAllModels(dapHandle);
                break;

            // Not yet supported, send error.
            case DAP_REMOVE_MODEL_COMMAND:
            case DAP_START_MODEL_UPLOAD_COMMAND:
            case DAP_END_MODEL_UPLOAD_COMMAND:
                break;

            case DAP_START_STREAMING_COMMAND:
                DAP_startStreaming(dapHandle);
                break;

            case DAP_STOP_STREAMING_COMMAND:
                DAP_stopStreaming(dapHandle);
                break;

            case DAP_LIST_INFERENCING_VALUES_COMMAND:
                DAP_listAllInferences(dapHandle);
                break;

            case DAP_READ_PROPERTY_COMMAND:
                index = commandFrame->buffer[LEN_IDX + DAP_READ_PROPERTY_CMD_FIXED_LENGHT];
                DAP_readProperty(dapHandle, index);
                break;

            case DAP_WRITE_PROPERTY_COMMAND:
                DAP_writeProperty(dapHandle, commandFrame);
                break;

            case DAP_LIST_PROPERTIES_COMMAND:
                DAP_listAllPropeties(dapHandle);
                break;

            default:
                break;
        }
    }
    else
    {
        DAP_sendDataCommand(dapHandle, commandFrame);
    }
}

void DAP_sendError(DAP_Handle *dapHandle, DAP_CommandType command, const char errorMessage[])
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_ERROR_COMMAND);

    payloadVariableLenght = strlen(errorMessage);
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_ERROR_FIXED_LENGHT + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = command;
    responseFrame.buffer[responseFrame.index++] = ERROR_NUMBER;

    DAP_preparePayload(&responseFrame, errorMessage, payloadVariableLenght);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

void DAP_receiveDataCommand(DAP_Handle *dapHandle, DAP_SendDataType sendDataType, void *data, size_t dataSize)
{
    if (!dapHandle->startStreamingFlag)
    {
        DAP_sendError(dapHandle, DAP_ERROR_COMMAND, "Not ready for streaming yet");
        return;
    }

    DAP_Frame responseFrame;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, (uint8_t)DAP_RECEIVE_DATA_COMMAND | (uint8_t)sendDataType);

    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_DATA_FIXED_LENGTH + dataSize);

    DAP_preparePayload(&responseFrame, data, dataSize);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_getCapabilities(DAP_Handle *dapHandle)
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_GET_CAPABILITIES_COMMAND);

    payloadVariableLenght = strlen(Device_name);
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_GET_CAPABILITIES_FIXED_LENGHT + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = DAP_API_VERSION;
    responseFrame.buffer[responseFrame.index++] = CAPABILITIES_FLAG;
    responseFrame.buffer[responseFrame.index++] = SDK_version[0];
    responseFrame.buffer[responseFrame.index++] = SDK_version[1];

    DAP_preparePayload(&responseFrame, Device_name, payloadVariableLenght);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_listAllSensors(DAP_Handle *dapHandle)
{
    for (uint8_t sensorIndex = 0; sensorIndex < availableSensorsCount; sensorIndex++)
    {
        DAP_listSensor(dapHandle, availableSensorsCount, sensorIndex);
    }
}

static void DAP_listSensor(DAP_Handle *dapHandle, uint8_t sensorCount, uint8_t sensorIndex)
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_LIST_SENSORS_COMMAND);

    payloadVariableLenght = (sensorCount > 0) ? strlen(sensorInformation[sensorIndex]) : 0;
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_LIST_SENSORS_FIXED_LENGHT + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = sensorCount;
    responseFrame.buffer[responseFrame.index++] = sensorIndex;

    if (sensorCount > 0)
    {
        DAP_preparePayload(&responseFrame, sensorInformation[sensorIndex], payloadVariableLenght);
    }

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_configurePipeline(DAP_Handle *dapHandle, DAP_Frame *frame)
{
    DAP_Frame responseFrame;
    uint8_t commandPayloadLenght;

    commandPayloadLenght = frame->buffer[LEN_IDX];

    dapHandle->pipelineConfig.mode                = (DAP_PipelineMode)frame->buffer[LEN_IDX + 1];
    dapHandle->pipelineConfig.inferenceModelIndex = frame->buffer[LEN_IDX + 2];

    for (uint8_t i = 0; i <= (commandPayloadLenght - DAP_CONFIGURE_PIPELINE_CMD_FIXED_LENGHT); i++)
    {
        dapHandle->pipelineConfig
            .inferenceSensorsIndex[i] = frame->buffer[LEN_IDX + DAP_CONFIGURE_PIPELINE_CMD_FIXED_LENGHT + 1 + i];
    }

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_CONFIGURE_PIPELINE_COMMAND);

    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_CONFIGURE_PIPELINE_FIXED_LENGHT);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_listAllModels(DAP_Handle *dapHandle)
{
    for (uint8_t modelIndex = 0; modelIndex < availableModelsCount; modelIndex++)
    {
        DAP_listModel(dapHandle, availableModelsCount, modelIndex);
    }
}

static void DAP_listModel(DAP_Handle *dapHandle, uint8_t modelCount, uint8_t modelIndex)
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_LIST_MODELS_COMMAND);

    payloadVariableLenght = (modelCount > 0) ? strlen(modelInformation[modelIndex]) : 0;
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_LIST_MODELS_FIXED_LENGHT + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = modelCount;
    responseFrame.buffer[responseFrame.index++] = modelIndex;

    if (modelCount > 0)
    {
        DAP_preparePayload(&responseFrame, modelInformation[modelIndex], payloadVariableLenght);
    }

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_startStreaming(DAP_Handle *dapHandle)
{
    dapHandle->startStreamingFlag = true;

    DAP_Frame responseFrame;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_START_STREAMING_COMMAND);

    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_START_STREAMING_FIXED_LENGHT);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_stopStreaming(DAP_Handle *dapHandle)
{
    dapHandle->startStreamingFlag = false;

    DAP_Frame responseFrame;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_STOP_STREAMING_COMMAND);

    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_STOP_STREAMING_FIXED_LENGHT);

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_listAllInferences(DAP_Handle *dapHandle)
{
    for (uint8_t inferenceIndex = 0; inferenceIndex < inferencesCount; inferenceIndex++)
    {
        DAP_listInference(dapHandle, inferencesCount, inferenceIndex);
    }
}

static void DAP_listInference(DAP_Handle *dapHandle, uint8_t inferenceCount, uint8_t inferenceIndex)
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_LIST_INFERENCING_VALUES_COMMAND);

    payloadVariableLenght = (inferenceCount > 0) ? strlen(inferences[inferenceIndex].info) : 0;
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_LIST_INFERENCING_VALUES_FIXED_LENGTH + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = inferenceCount;
    responseFrame.buffer[responseFrame.index++] = inferenceIndex;
    responseFrame.buffer[responseFrame.index++] = inferences[inferenceIndex].dataFormat;

    if (inferenceCount > 0)
    {
        DAP_preparePayload(&responseFrame, inferences[inferenceIndex].info, payloadVariableLenght);
    }

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_readProperty(DAP_Handle *dapHandle, uint8_t index)
{
    if (index > propertiesCount)
    {
        DAP_sendError(dapHandle, DAP_ERROR_COMMAND, "Property index too high.");
        return;
    }

    DAP_Frame responseFrame;
    DAP_DataTypes dataType = properties[index].dataFormat;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_READ_PROPERTY_COMMAND);

    switch (dataType)
    {
        case DAP_DATA_TYPE_UINT16:
            // Add two bytes due to dataType
            DAP_prepareLengthByte(&responseFrame, (size_t)DAP_READ_PROPERTY_FIXED_LENGHT + 2);

            responseFrame.buffer[responseFrame.index++] = index;

            // Send value in bytes, MSB first
            responseFrame.buffer[responseFrame.index++] = (properties[index].value.u16 >> 8) & 0xFF;
            responseFrame.buffer[responseFrame.index++] = properties[index].value.u16 & 0xFF;
            break;

        case DAP_DATA_TYPE_UINT32:
            // Add four bytes due to dataType
            DAP_prepareLengthByte(&responseFrame, (size_t)DAP_READ_PROPERTY_FIXED_LENGHT + 4);

            responseFrame.buffer[responseFrame.index++] = index;

            // Send value in bytes, MSB first
            responseFrame.buffer[responseFrame.index++] = (properties[index].value.u32 >> 24) & 0xFF;
            responseFrame.buffer[responseFrame.index++] = (properties[index].value.u32 >> 16) & 0xFF;
            responseFrame.buffer[responseFrame.index++] = (properties[index].value.u32 >> 8) & 0xFF;
            responseFrame.buffer[responseFrame.index++] = properties[index].value.u32 & 0xFF;
            break;

        default:
            DAP_sendError(dapHandle, DAP_ERROR_COMMAND, "Property data type not supported yet.");
            break;
    }

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_writeProperty(DAP_Handle *dapHandle, DAP_Frame *frame)
{
    uint8_t index;
    DAP_DataTypes dataType;
    uint8_t byte[8];
    DAP_Frame responseFrame;

    index    = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT];
    dataType = properties[index].dataFormat;

    if (index > propertiesCount)
    {
        DAP_sendError(dapHandle, DAP_ERROR_COMMAND, "Property index too high.");
        return;
    }

    switch (dataType)
    {
        case DAP_DATA_TYPE_UINT16:
            byte[0]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 1];
            byte[1]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 2];
            properties[index].value.u16 = ((byte[0] << 8) | byte[1]);
            break;

        case DAP_DATA_TYPE_UINT32:
            byte[0]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 1];
            byte[1]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 2];
            byte[2]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 3];
            byte[3]                     = frame->buffer[LEN_IDX + DAP_WRITE_PROPERTY_CMD_FIXED_LENGHT + 4];
            properties[index].value.u32 = ((byte[0] << 24) | (byte[1] << 16) | (byte[2] << 8) | byte[3]);
            break;

        default:
            DAP_sendError(dapHandle, DAP_ERROR_COMMAND, "Property data type not supported yet.");
            break;
    }

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_WRITE_PROPERTY_COMMAND);

    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_WRITE_PROPERTY_FIXED_LENGHT);

    responseFrame.buffer[responseFrame.index++] = index;

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_listAllPropeties(DAP_Handle *dapHandle)
{
    for (uint8_t propertyIndex = 0; propertyIndex < propertiesCount; propertyIndex++)
    {
        DAP_listProperty(dapHandle, propertiesCount, propertyIndex);
    }
}

static void DAP_listProperty(DAP_Handle *dapHandle, uint8_t propertyCount, uint8_t propertyIndex)
{
    DAP_Frame responseFrame;
    size_t payloadVariableLenght;

    /* Prepare Tx Header */
    DAP_prepareTxHeader(&responseFrame, DAP_LIST_PROPERTIES_COMMAND);

    payloadVariableLenght = (propertyCount > 0) ? strlen(properties[propertyIndex].name) : 0;
    DAP_prepareLengthByte(&responseFrame, (size_t)DAP_LIST_PROPERTIES_FIXED_LENGHT + payloadVariableLenght);

    responseFrame.buffer[responseFrame.index++] = propertyCount;
    responseFrame.buffer[responseFrame.index++] = propertyIndex;
    responseFrame.buffer[responseFrame.index++] = properties[propertyIndex].dataFormat;

    if (propertyCount > 0)
    {
        DAP_preparePayload(&responseFrame, properties[propertyIndex].name, payloadVariableLenght);
    }

    /* Prepare Tx Trailer */
    DAP_prepareTxTrailer(&responseFrame, dapHandle->isCrc);

    MessageQueueP_post(dapHandle->messageQueueHandle, &responseFrame, MessageQueueP_WAIT_FOREVER);
}

static void DAP_sendDataCommand(DAP_Handle *dapHandle, DAP_Frame *frame)
{
    DAP_sendError(dapHandle, DAP_SEND_DATA_COMMAND, "Receiving data not yet supported");

    // We are told by a previous command (TBD which one) when to start uploading data
    // if (startUploadFlag)
    // {

    // }

    // We don't need to send an acknowledge to this command.
}

static void DAP_prepareTxHeader(DAP_Frame *frame, DAP_CommandType cmd)
{
    frame->index = 0;

    /* Add Start Byte */
    frame->buffer[frame->index++] = FRAME_START_BYTE;

    /* Add Command Byte */
    frame->buffer[frame->index++] = cmd;
}

static void DAP_prepareTxTrailer(DAP_Frame *frame, bool isCrc)
{
    // if(isCrc)
    // {
    //     /* Calculate CRC16 */
    //     /* From Start byte to end of data byte */
    //     uint8_t size = txMsg->ptr - PREAMBLE_BYTE_SIZE;
    //     /* Send address of startbyte */
    //     uint16_t checkSum = CRC_calc16(&txMsg->buffer[PREAMBLE_BYTE_SIZE],size);

    //     /* Add Checksum */
    //     u8From16(&txMsg->buffer[txMsg->ptr],checkSum);
    //     txMsg->ptr += 2;
    // }

    /* Add End byte */
    frame->buffer[frame->index++] = FRAME_END_BYTE;

    frame->length = frame->index;
}

static void DAP_prepareLengthByte(DAP_Frame *frame, size_t payloadLength)
{
    uint8_t byte1, byte2, byte3;

    if (payloadLength <= 0x7f)
    {
        frame->buffer[frame->index++] = payloadLength;
    }
    else if ((payloadLength > 0x7F) && (payloadLength <= 0x3FF))
    {
        // Set MSB of the second byte
        payloadLength |= 0x8000;

        // Divide into two bytes
        byte1 = (payloadLength >> 8) & 0xFF;
        byte2 = payloadLength & 0xFF;

        // Store MSB first
        frame->buffer[frame->index++] = byte1;
        frame->buffer[frame->index++] = byte2;
    }
    else if ((payloadLength > 0x3FF) && (payloadLength <= 0x3FFFFF))
    {
        // Set two MSBs of the third byte
        payloadLength |= 0xC00000;

        // Divide into three bytes
        byte1 = (payloadLength >> 16) & 0xFF;
        byte2 = (payloadLength >> 8) & 0xFF;
        byte3 = payloadLength & 0xFF;

        // Store MSB first
        frame->buffer[frame->index++] = byte1;
        frame->buffer[frame->index++] = byte2;
        frame->buffer[frame->index++] = byte3;
    }
}

static void DAP_preparePayload(DAP_Frame *frame, const void *data, size_t dataLength)
{
    /* Compute offset for the payload */
    uint8_t offset = frame->index;

    /* Check if the buffer is too small */
    if (offset + dataLength > DAP_MAX_FRAME_SIZE_BYTES)
    {
        while (1) {}
    }

    /* Place payload in frame considering the offset */
    memcpy(((uint8_t *)frame->buffer + offset), data, dataLength);

    /* Update offset with payload length. */
    frame->index += (uint8_t)dataLength;
}
