#include "cantp_core.h"
#include "cantp.h"
#include "cantp_types.h"
#include "cantp_hal.h"
#include "cantp_cfg.h"

#define IsLastCF(nodePtr) (nodePtr->cnt == nodePtr->allCnt)
#define IsSNError(nodePtr, SN) ((nodePtr->cnt % 0x10) != SN )
#define IsNumError(nodePtr) (nodePtr->bytesNums > nodePtr->allBytesNum)

typedef enum
{
    CANTP_CORE_RX_WAIT,                             /**< Rx wait */
    CANTP_CORE_RX_SF,                               /**< Rx SF */
    CANTP_CORE_RX_FF,                               /**< Rx FF */
    CANTP_CORE_TX_FC,                               /**< Tx FC */
    CANTP_CORE_TX_FC_WAIT,                          /**< Wait FC confirm */
    CANTP_CORE_RX_CF,                               /**< Rx CF */
    CANTP_CORE_RX_ABORT_WAIT,                       /**< Rx abort, waiting for Br timeout */
    CANTP_CORE_RX_ABORT,                            /**< Rx abort */
    CANTP_CORE_RX_WAIT_BR,                          /**< Wait Br */

    CANTP_CORE_TX_WAIT,                             /**< Tx wait */
    CANTP_CORE_TX_SF,                               /**< Tx SF */
    CANTP_CORE_TX_SF_WAIT,                          /**< Wait SF confirm */
    CANTP_CORE_TX_FF,                               /**< Tx FF */
    CANTP_CORE_TX_FF_WAIT,                          /**< Wait FF confirm */
    CANTP_CORE_TX_CF,                               /**< Tx CF */
    CANTP_CORE_TX_CF_WAIT,                          /**< Wait CF confirm */
    CANTP_CORE_RX_FC,                               /**< Rx FC */
    CANTP_CORE_TX_WAIT_CS,                          /**< Wait Cs */
} cantp_core_task_status_type;

typedef enum
{
    CANTP_CORE_FS_CTS,                              /**< Continue to send */
    CANTP_CORE_FS_WAIT,                             /**< Wait */
    CANTP_CORE_FS_OVFLW,                            /**< Fifo overflow */
} cantp_core_fs_type;

typedef struct cantp_core_task_node_type_t cantp_core_task_node_type_t;

typedef void (*cantp_core_tx_func_ptr_type)(cantp_core_task_node_type_t* nodePtr);

typedef void (*cantp_core_rx_func_ptr_type)(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr);

typedef void (*cantp_core_confirm_fun_type)(cantp_core_task_node_type_t* nodePtr);
struct cantp_core_task_node_type_t {
    cantp_u8 sduId;                                 /**< Sdu id of current node */
    cantp_u8 STmin;                                 /**< STmin */
    cantp_core_task_status_type status;             /**< Status */
    cantp_u8 CANTP_DL;                              /**< TX_DL or RX_DL */
    cantp_u8 BSCnt;                                 /**< Block size */
    cantp_bool ctsFlag;                             /**< Block size */
    cantp_u8 waitCnt;                               /**< Wait for FC count */
    cantp_u16 cnt;                                  /**< Frame num has been done */
    cantp_u16 allCnt;                               /**< All frame num need to be done */
    cantp_u16 bytesNums;                            /**< Byte num need to be Tx or Rx */
    cantp_u16 allBytesNum;                          /**< All bytes num need to be Tx or Rx */
    cantp_time_type waitTime;                       /**< Wait time */
};


/* ============================================================================================== */
/*                                 CANTP CORE FUNCTION DECLARATION                                */
/* ============================================================================================== */
/* The fellowing function is used to receive msg */
void CanTp_Core_RxSF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr);
void CanTp_Core_RxFF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr);
void CanTp_Core_RxCF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr);
void CanTp_Core_TxFC(cantp_u16 sduId);
void CanTp_Core_ConfirmFc(cantp_core_task_node_type_t* nodePtr);

/* The fellowing function is used to transmit msg */
void CanTp_Core_TxSF(sdu_id_type sduId, cantp_u32 pendingLength);
void CanTp_Core_TxFF(sdu_id_type sduId, cantp_u32 pendingLength);
void CanTp_Core_TxCF(sdu_id_type sduId, cantp_u32 pendingLength);
void CanTp_Core_RxFC(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr);


cantp_core_task_node_type_t CanTp_Core_txNodeArray[CANTP_CHANNEL_NUM];
cantp_core_task_node_type_t CanTp_Core_rxNodeArray[CANTP_CHANNEL_NUM];

const cantp_u8* CanTp_Core_pciOffsetArray;
const cantp_time_array_type_t* CanTp_Core_timeArrayPtr;
const cantp_ch_config_array_type_t* CanTp_Core_channelArrayPtr;

void CanTp_Core_Init(const cantp_config_t* configPtr)
{
    CanTp_Core_timeArrayPtr = configPtr->timeMap;
    CanTp_Core_channelArrayPtr = configPtr->chConfig;
    CanTp_Core_pciOffsetArray = configPtr->pciOffsetArray;
    for (cantp_u8 idx = 0U; idx < CANTP_CHANNEL_NUM; idx++)
    {
        CanTp_Core_txNodeArray[idx].sduId = idx;
        CanTp_Core_txNodeArray[idx].CANTP_DL = configPtr->chConfig[idx].TX_DL;
        CanTp_Core_txNodeArray[idx].status = CANTP_CORE_TX_WAIT;
        CanTp_Core_txNodeArray[idx].waitTime = 0;
    }

    for (cantp_u8 idx = 0U; idx < CANTP_CHANNEL_NUM; idx++)
    {
        CanTp_Core_rxNodeArray[idx].sduId = idx;
        CanTp_Core_rxNodeArray[idx].CANTP_DL = 0;
        CanTp_Core_rxNodeArray[idx].status = CANTP_CORE_RX_WAIT;
        CanTp_Core_rxNodeArray[idx].waitTime = 0;
    }
}

/* ============================================================================================== */
/*                                      RX RELATIVE FUNCTIONS                                     */
/* ============================================================================================== */
void CanTp_Core_RxSF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr)
{
    do
    {
        nodePtr->status = CANTP_CORE_RX_SF;
        if (pduInfoPtr->sduLength < 2)
        {
            /* The min len will be: 1 byte PCI + 1 byte Data */
            break;
        }
        pdu_length_type SF_DL;
        pdu_info_type_t tempPduInfo;
        tempPduInfo.metaDataPtr = pduInfoPtr->metaDataPtr;
        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[nodePtr->sduId];
        /* PCI offset */
        cantp_u8 MAX_SF_DL = 0;
        cantp_u8 MIN_SF_DL = 0;
        cantp_u8 RX_DL = pduInfoPtr->sduLength + pciOffset;
        cantp_bool isPadding = CanTp_Core_channelArrayPtr[nodePtr->sduId].padding;
        if (RX_DL > 8)
        {
            if(pduInfoPtr->sduDataPtr[0] != 0x00U)
            {
                break;
            }
            tempPduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 2);
            SF_DL = pduInfoPtr->sduDataPtr[1];
            MAX_SF_DL = pduInfoPtr->sduLength - 2U; /* PCI + address */;
            MIN_SF_DL = CanTp_Hal_GetRxMinLength(RX_DL) - pciOffset;
        }
        else if (RX_DL == 8)
        {
            tempPduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 1);
            SF_DL = pduInfoPtr->sduDataPtr[0] & 0x0F;
            MAX_SF_DL = pduInfoPtr->sduLength - 1;
            if (isPadding == CANTP_TRUE)
            {
                MIN_SF_DL = 1;
            }
            else
            {
                MIN_SF_DL = pduInfoPtr->sduLength - 1;
            }
        }
        else
        {
            if (isPadding == CANTP_TRUE)
            {
                break;
            }
            tempPduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 1);
            SF_DL = pduInfoPtr->sduDataPtr[0] & 0x0F;
            MAX_SF_DL = pduInfoPtr->sduLength - 1;
            MIN_SF_DL = pduInfoPtr->sduLength - 1;
        }
        /* Check wether the SF_DL is valid */
        if ((SF_DL < MIN_SF_DL) || (SF_DL > MAX_SF_DL))
        {
            break;
        }
        tempPduInfo.sduLength = SF_DL;
        cantp_u32 bufferSize;
        buf_req_return_type bufRes = CanTp_StartOfReception(nodePtr->sduId, &tempPduInfo, SF_DL, &bufferSize);

        if (bufferSize < SF_DL)
        {
            /** [SWS_CanTp_Core_00339] */
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_BUFFER_OVFLW);
            break;
        }

        /* CANTP_BUF_REQ_NOT_OK and CANTP_BUF_REQ_BUSY */
        if (CANTP_BUF_REQ_OK != bufRes)
        {
            /** [SWS_CanTp_Core_00081] */
            /** [SWS_CanTp_Core_00353] */
            break;
        }

        bufRes = CanTp_CopyRxData(nodePtr->sduId, &tempPduInfo, &bufferSize);
        CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_OK);
    } while (CANTP_FALSE);
    nodePtr->status = CANTP_CORE_RX_WAIT;
}

void CanTp_Core_RxFF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr)
{
    do
    {
        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[nodePtr->sduId];
        if (pduInfoPtr->sduLength + pciOffset < 8)
        {
            /* Receive a FF with CAN_DL < 8*/
            break;
        }
        cantp_u32 MIN_FF_DL = pduInfoPtr->sduLength;
        if (pduInfoPtr->sduLength + pciOffset > 8)
        {
            MIN_FF_DL -= 1;
        }
        pdu_length_type FF_DL = ((pduInfoPtr->sduDataPtr[0] << 8) + pduInfoPtr->sduDataPtr[1]) & 0x0FFF;
        pdu_length_type currentLen;
        pdu_info_type_t tempSduInfo;
        tempSduInfo.metaDataPtr = pduInfoPtr->metaDataPtr;
        tempSduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 2);
        currentLen = pduInfoPtr->sduLength - 2;
        if (FF_DL == 0U)
        {
            MIN_FF_DL = 4096;
            FF_DL = (pduInfoPtr->sduDataPtr[2] << 24) +
                    (pduInfoPtr->sduDataPtr[3] << 16) +
                    (pduInfoPtr->sduDataPtr[4] << 8) +
                    pduInfoPtr->sduDataPtr[5];
            tempSduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 6);
            currentLen = pduInfoPtr->sduLength - 6;
        }
        if (FF_DL < MIN_FF_DL)
        {
            break;
        }
        tempSduInfo.sduLength = FF_DL;
        nodePtr->status = CANTP_CORE_RX_FF;
        nodePtr->CANTP_DL = pduInfoPtr->sduLength;

        /** [SWS_CanTp_Core_00166] */
        nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBr;

        cantp_u32 bufferSize;
        buf_req_return_type bufRes = CanTp_StartOfReception(nodePtr->sduId, &tempSduInfo, FF_DL, &bufferSize);

        if (CANTP_BUF_REQ_OVFL == bufRes)
        {
            /* fifo overflow */
            /* SWS_CanTp_00318: Tx FC and abort */
            nodePtr->status = CANTP_CORE_RX_ABORT_WAIT;
            nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBr;
            // CanTp_Core_TxFC(nodePtr->sduId);
            break;
        }

        /* CANTP_BUF_REQ_NOT_OK and CANTP_BUF_REQ_BUSY */
        if (CANTP_BUF_REQ_OK != bufRes)
        {
            /** [SWS_CanTp_Core_00081] Buffer not ok will ignore */
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        }

        if (bufferSize < currentLen)
        {
            /** [SWS_CanTp_00339] */
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_BUFFER_OVFLW);
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        }

        nodePtr->cnt = 1;
        nodePtr->allCnt = 1 + (FF_DL - currentLen + nodePtr->CANTP_DL - 2) / (nodePtr->CANTP_DL - 1);
        nodePtr->bytesNums = currentLen;
        nodePtr->allBytesNum = FF_DL;
        nodePtr->BSCnt = CanTp_Core_channelArrayPtr[nodePtr->sduId].bs;
        if (currentLen != 0)
        {
            tempSduInfo.sduLength = currentLen;
            bufRes = CanTp_CopyRxData(nodePtr->sduId, &tempSduInfo, &bufferSize);
        }
        nodePtr->status = CANTP_CORE_RX_WAIT_BR;
    } while (CANTP_FALSE);
}

void CanTp_Core_TxFC(cantp_u16 sduId)
{
    /** TODO after fifo uodate
    pdu_length_type bufferSize;
    CanTp_CopyRxData(sduId, CANTP_NULL, &bufferSize);
    */
    cantp_hal_tx_res_type res = CanTp_Hal_GetMBStatus(sduId);
    if (res == CANTP_HAL_TX_WAIT)
    {
        cantp_core_fs_type fs;
        if (CanTp_Core_rxNodeArray[sduId].status == CANTP_CORE_RX_ABORT)
        {
            fs = CANTP_CORE_FS_OVFLW;
        }
        else
        {
            /* Current fifo will alway be ok once started reception */
            fs = CANTP_CORE_FS_CTS;
        }
        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[sduId];
        cantp_u8 fcData[8] = {0};
        if (pciOffset == 1U)
        {
            fcData[0] = CanTp_Core_channelArrayPtr[sduId].addrByte;
            fcData[1] = 0x30 + fs;
            fcData[2] = CanTp_Core_channelArrayPtr[sduId].bs;
            fcData[3] = CanTp_Core_timeArrayPtr[sduId].stMin;
        }
        else
        {
            fcData[0] = 0x30 + fs;
            fcData[1] = CanTp_Core_channelArrayPtr[sduId].bs;
            fcData[2] = CanTp_Core_timeArrayPtr[sduId].stMin;
        }
        pdu_info_type_t tempPdu = {
            .metaDataPtr = CANTP_NULL,
            .sduDataPtr = fcData,
            .sduLength = 3 + pciOffset,
        };
        if (CanTp_Core_channelArrayPtr[sduId].padding == CANTP_ON) {
            for (int idx = tempPdu.sduLength; idx < CANTP_TX_DL_CHANGE_NUM; idx++)
            {
                tempPdu.sduDataPtr[idx] = CanTp_Core_channelArrayPtr[sduId].paddingValue;
            }
            tempPdu.sduLength = CANTP_TX_DL_CHANGE_NUM;
        }
        if (CanTp_Core_rxNodeArray[sduId].status != CANTP_CORE_RX_ABORT)
        {
            CanTp_Core_rxNodeArray[sduId].status = CANTP_CORE_TX_FC_WAIT;
        }
        CanTp_Core_rxNodeArray[sduId].BSCnt = CanTp_Core_channelArrayPtr[sduId].bs;
        CanTp_Core_rxNodeArray[sduId].waitTime = CanTp_Core_timeArrayPtr[sduId].nAr;
        (void)CanTp_Hal_Transmit(sduId * 2 + 1, &tempPdu);
    }
}

void CanTp_Core_RxCF(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr)
{
    do {
        pdu_length_type CF_DL = pduInfoPtr->sduLength - 1;

        cantp_u8 SN = pduInfoPtr->sduDataPtr[0] & 0x0F;
        if (IsSNError(nodePtr, SN))
        {
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_WRONG_SN);
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        }
        /* add rx cnt to check whether this frame is the last frame */
        nodePtr->cnt += 1;
        if (IsLastCF(nodePtr))
        {
            cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[nodePtr->sduId];
            cantp_u8 leftByteNum = nodePtr->allBytesNum - nodePtr->bytesNums;
            cantp_u8 TX_DL = CanTp_Hal_GetTxLength(leftByteNum + pciOffset + 1);
            CF_DL = leftByteNum;
            if (TX_DL < 8)
            {
                if ((CanTp_Core_channelArrayPtr[nodePtr->sduId].padding == CANTP_TRUE) && (pduInfoPtr->sduLength + pciOffset != 8U))
                {
                    nodePtr->cnt -= 1;
                    break;
                }
                if ((CanTp_Core_channelArrayPtr[nodePtr->sduId].padding == CANTP_FALSE) && (TX_DL != pduInfoPtr->sduLength + pciOffset))
                {
                    /* According to ISO 15765-2: 2016 Figure 9, ignore the wrong CF_DL frame */
                    nodePtr->cnt -= 1;
                    break;
                }
            }
            else
            {
                if (TX_DL != pduInfoPtr->sduLength + pciOffset)
                {
                    nodePtr->cnt -= 1;
                    break;
                }
            }
        }
        else if (CF_DL != nodePtr->CANTP_DL - 1)
        {
            nodePtr->cnt -= 1;
            break;
        }
        nodePtr->bytesNums += CF_DL;
        pdu_info_type_t tempPduInfo;
        tempPduInfo.metaDataPtr = pduInfoPtr->metaDataPtr;
        tempPduInfo.sduDataPtr = (pduInfoPtr->sduDataPtr + 1);
        tempPduInfo.sduLength = CF_DL;
        pdu_length_type bufferSize;
        buf_req_return_type res = CanTp_CopyRxData(nodePtr->sduId, &tempPduInfo, &bufferSize);

        if (res == CANTP_BUF_REQ_NOT_OK)
        {
            /* Buf Overflow */
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_BUFFER_OVFLW);
            nodePtr->status = CANTP_CORE_RX_WAIT;
        }

        if (IsLastCF(nodePtr))
        {
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_OK);
            nodePtr->status = CANTP_CORE_RX_WAIT;
        }
        else
        {
            if (nodePtr->BSCnt == 1U)
            {
                nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBr;
                /* Change the block size counter in tx fc function. */
                nodePtr->status = CANTP_CORE_RX_WAIT_BR;
            }
            else
            {
                nodePtr->BSCnt--;
                nodePtr->status = CANTP_CORE_RX_CF;
                nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nCr;
            }
        }
    } while (CANTP_FALSE);
}

void CanTp_Core_ConfirmFc(cantp_core_task_node_type_t* nodePtr)
{
    do {
        // Abort
        if (nodePtr->status == CANTP_CORE_RX_ABORT)
        {
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        }
        else if (nodePtr->status != CANTP_CORE_TX_FC_WAIT)
        {
            break;
        }

        nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nCr;
        nodePtr->status = CANTP_CORE_RX_CF;
    } while (CANTP_FALSE);
}

/* ============================================================================================== */
/*                                      TX RELATIVE FUNCTIONS                                     */
/* ============================================================================================== */
void CanTp_Core_TxNode(sdu_id_type sduId)
{
    cantp_core_task_node_type_t* nodePtr = &CanTp_Core_txNodeArray[sduId];
    cantp_u32 pendingLength = 0U;
    switch (nodePtr->status)
    {
        case CANTP_CORE_TX_WAIT:
            pendingLength = CanTp_GetPendingLength(sduId);
            /* this stage is needed to fix continue tx bug */
            if (pendingLength > CanTp_Core_channelArrayPtr[sduId].SF_DL)
            {
                nodePtr->status = CANTP_CORE_TX_FF;
                nodePtr->waitTime = CanTp_Core_timeArrayPtr[sduId].nAs;
            }
            /* pending length may be 0 */
            else if (pendingLength > 0)
            {
                nodePtr->status = CANTP_CORE_TX_SF;
                nodePtr->waitTime = CanTp_Core_timeArrayPtr[sduId].nAs;
            }
            break;
        case CANTP_CORE_TX_SF:
            pendingLength = CanTp_GetPendingLength(sduId);
            CanTp_Core_TxSF(sduId, pendingLength);
            break;
        case CANTP_CORE_TX_FF:
            pendingLength = CanTp_GetPendingLength(sduId);
            CanTp_Core_TxFF(sduId, pendingLength);
            break;
        case CANTP_CORE_TX_CF:
            pendingLength = CanTp_GetPendingLength(sduId);
            /** if some error happens, the pendingLength may be 0 */
            if (pendingLength > 0)
            {
                CanTp_Core_TxCF(sduId, pendingLength);
            }
            break;
        default:
            break;
    }
}

void CanTp_Core_TxSF(sdu_id_type sduId, cantp_u32 pendingLength)
{
    cantp_hal_tx_res_type res = CanTp_Hal_GetMBStatus(sduId);
    if (res == CANTP_HAL_TX_WAIT)
    {
        cantp_u8 dataBuf[MAX_TX_DL] = {0U};

        /* This Pdu is used to transmit to lower layer */
        pdu_info_type_t tempPdu = {
            .metaDataPtr = CANTP_NULL,
            .sduDataPtr = dataBuf,
            .sduLength = pendingLength,
        };

        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[sduId];
        cantp_u8 extraDataLen = 1;
        if (pciOffset == 1U)
        {
            dataBuf[0] = CanTp_Core_channelArrayPtr[sduId].addrByte;
            extraDataLen += 1;
        }
        cantp_u8* pciPtr = dataBuf + pciOffset;
        if (pciOffset + 1 + pendingLength > 8)
        {
            pciPtr[0] = 0x00;
            pciPtr[1] = (cantp_u8)pendingLength;
            tempPdu.sduDataPtr = pciPtr + 2;
            extraDataLen += 1;
        }
        else
        {
            pciPtr[0] = 0x00 + pendingLength;
            tempPdu.sduDataPtr = pciPtr + 1;

        }
        retry_info_type_t retryInfo = {
            .tpDataState = CANTP_DATA_CONF,
            .txTpDataCnt = 0,
        };
        CanTp_CopyTxData(sduId, &tempPdu, &retryInfo, CANTP_NULL);
        tempPdu.sduLength += extraDataLen;
        tempPdu.sduDataPtr = dataBuf;
        if (CanTp_Core_channelArrayPtr[sduId].padding == CANTP_TRUE &&
            tempPdu.sduLength < CANTP_TX_DL_CHANGE_NUM
        ) {
            const cantp_u8 paddingValue = CanTp_Core_channelArrayPtr[sduId].paddingValue;
            for (int idx = tempPdu.sduLength; idx < CANTP_TX_DL_CHANGE_NUM; idx++)
            {
                tempPdu.sduDataPtr[idx] = paddingValue;
            }
            tempPdu.sduLength = CANTP_TX_DL_CHANGE_NUM;
        }
        CanTp_Core_txNodeArray[sduId].status = CANTP_CORE_TX_SF_WAIT;
        CanTp_Core_txNodeArray[sduId].waitTime = CanTp_Core_timeArrayPtr[sduId].nAs;
        (void)CanTp_Hal_Transmit(sduId * 2, &tempPdu);
    }
}

void CanTp_Core_TxFF(sdu_id_type sduId, cantp_u32 pendingLength)
{
    cantp_hal_tx_res_type res = CanTp_Hal_GetMBStatus(sduId);
    if (res == CANTP_HAL_TX_WAIT)
    {
        cantp_u8 dataBuf[MAX_TX_DL] = {0U};

        cantp_core_task_node_type_t* nodePtr = &CanTp_Core_txNodeArray[sduId];

        /* This Pdu is used to transmit to lower layer */
        pdu_info_type_t tempPdu = {
            .metaDataPtr = CANTP_NULL,
            .sduDataPtr = dataBuf,
            .sduLength = 0,
        };

        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[sduId];
        cantp_u8 extraDataLen = 0;
        if (pciOffset == 1U)
        {
            dataBuf[0] = CanTp_Core_channelArrayPtr[sduId].addrByte;
            extraDataLen += 1;
        }

        cantp_u8* pciPtr = dataBuf + pciOffset;
        if (pendingLength > 0x0FFFU)
        {
            /* data len greater than 4095 */
            pciPtr[0] = 0x10;
            pciPtr[1] = 0x00;
            pciPtr[2] = pendingLength >> 24;
            pciPtr[3] = (pendingLength >> 16) & 0xFF;
            pciPtr[4] = (pendingLength >> 8) & 0xFF;
            pciPtr[5] = pendingLength & 0xFF;
            tempPdu.sduDataPtr = pciPtr + 6;
            extraDataLen += 6;
        }
        else
        {
            pciPtr[0] = 0x10 + ((pendingLength & 0x0F00) >> 8);
            pciPtr[1] = pendingLength & 0x00FF;
            tempPdu.sduDataPtr = pciPtr + 2;
            extraDataLen += 2;
        }

        retry_info_type_t retryInfo = {
            .tpDataState = CANTP_DATA_CONF,
            .txTpDataCnt = 0,
        };
        tempPdu.sduLength = CanTp_Core_channelArrayPtr[sduId].TX_DL - extraDataLen;
        CanTp_CopyTxData(sduId, &tempPdu, &retryInfo, CANTP_NULL);
        tempPdu.sduLength += extraDataLen;
        tempPdu.sduDataPtr = dataBuf;
        nodePtr->status = CANTP_CORE_TX_FF_WAIT;
        nodePtr->waitTime = CanTp_Core_timeArrayPtr[sduId].nAs;

        nodePtr->allBytesNum = pendingLength;
        nodePtr->bytesNums = CanTp_Core_channelArrayPtr[sduId].TX_DL - extraDataLen;
        nodePtr->cnt = 1;

        (void)CanTp_Hal_Transmit(sduId * 2, &tempPdu);
    }
}

void CanTp_Core_TxCF(sdu_id_type sduId, cantp_u32 pendingLength)
{
    cantp_hal_tx_res_type res = CanTp_Hal_GetMBStatus(sduId);
    if (res == CANTP_HAL_TX_WAIT)
    {
        cantp_u8 dataBuf[MAX_TX_DL] = {0U};

        // cantp_core_task_node_type_t* nodePtr = &CanTp_Core_txNodeArray[sduId];

        /* This Pdu is used to transmit to lower layer */
        pdu_info_type_t tempPdu = {
            .metaDataPtr = CANTP_NULL,
            .sduDataPtr = dataBuf,
            .sduLength = 0,
        };

        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[sduId];
        cantp_u8 extraDataLen = 1;
        if (pciOffset == 1U)
        {
            dataBuf[0] = CanTp_Core_channelArrayPtr[sduId].addrByte;
            extraDataLen += 1;
        }

        cantp_u8* pciPtr = dataBuf + pciOffset;
        cantp_core_task_node_type_t* nodePtr = &CanTp_Core_txNodeArray[sduId];
        pciPtr[0] = 0x20 + (CanTp_Core_txNodeArray[sduId].cnt % 0x10);
        nodePtr->cnt++;
        nodePtr->BSCnt--;
        if (pendingLength > CanTp_Core_channelArrayPtr[sduId].TX_DL - extraDataLen)
        {
            tempPdu.sduLength = CanTp_Core_channelArrayPtr[sduId].TX_DL - extraDataLen;
            nodePtr->bytesNums += CanTp_Core_channelArrayPtr[sduId].TX_DL - extraDataLen;
        }
        else
        {
            tempPdu.sduLength = pendingLength;
            nodePtr->bytesNums += pendingLength;
        }
        tempPdu.sduDataPtr = pciPtr + 1;

        retry_info_type_t retryInfo = {
            .tpDataState = CANTP_DATA_CONF,
            .txTpDataCnt = 0,
        };

        CanTp_CopyTxData(sduId, &tempPdu, &retryInfo, CANTP_NULL);
        tempPdu.sduLength += extraDataLen;
        tempPdu.sduDataPtr = dataBuf;
        CanTp_Core_txNodeArray[sduId].status = CANTP_CORE_TX_CF_WAIT;
        if (nodePtr->bytesNums == nodePtr->allBytesNum && 
            CanTp_Core_channelArrayPtr[sduId].padding == CANTP_TRUE &&
            tempPdu.sduLength < CANTP_TX_DL_CHANGE_NUM)
        {
            const cantp_u8 paddingValue = CanTp_Core_channelArrayPtr[sduId].paddingValue;
            for (int idx = tempPdu.sduLength; idx < CANTP_TX_DL_CHANGE_NUM; idx++)
            {
                tempPdu.sduDataPtr[idx] = paddingValue;
            }
            tempPdu.sduLength = CANTP_TX_DL_CHANGE_NUM;
        }
        (void)CanTp_Hal_Transmit(sduId * 2, &tempPdu);
    }
}

void CanTp_Core_RxFC(cantp_core_task_node_type_t* nodePtr, const pdu_info_type_t* pduInfoPtr)
{
    do {
        /* Only `CANTP_CORE_RX_FC` can receive FC */
        if (nodePtr->status != CANTP_CORE_RX_FC)
        {
            break;
        }
        cantp_core_fs_type fs = pduInfoPtr->sduDataPtr[0] & 0x0F;
        switch (fs)
        {
            case CANTP_CORE_FS_CTS:
            {
                cantp_u8 blockSize = pduInfoPtr->sduDataPtr[1];
                cantp_u8 STmin = pduInfoPtr->sduDataPtr[2];
                if (blockSize == 0U)
                {
                    nodePtr->ctsFlag = CANTP_TRUE;
                }
                else
                {
                    nodePtr->ctsFlag = CANTP_FALSE;
                }
                nodePtr->BSCnt = blockSize;
                nodePtr->STmin = STmin;
                nodePtr->waitCnt = CanTp_Core_channelArrayPtr[nodePtr->sduId].maxWtf;
                nodePtr->status = CANTP_CORE_TX_WAIT_CS;
                nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nCs;
                break;
            }
            case CANTP_CORE_FS_WAIT:
            {
                nodePtr->waitCnt--;
                if (nodePtr->waitCnt)
                {
                    nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBs;
                }
                else
                {
                    CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_WFT_OVRN);
                    nodePtr->status = CANTP_CORE_TX_WAIT;
                }
                break;
            }
            case CANTP_CORE_FS_OVFLW:
            {
                CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_BUFFER_OVFLW);
                nodePtr->status = CANTP_CORE_TX_WAIT;
                break;
            }
            default:
            {
                CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_INVALID_FS);
                nodePtr->status = CANTP_CORE_TX_WAIT;
                break;
            }
        }

    } while (CANTP_FALSE);
}

/* ============================================================================================== */
/*                                         OTHER FUNCTIONS                                        */
/* ============================================================================================== */
void CanTp_Core_ConfirmTx(cantp_core_task_node_type_t* nodePtr)
{
    switch (nodePtr->status)
    {
        case CANTP_CORE_TX_SF_WAIT:
            CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_OK);
            nodePtr->status = CANTP_CORE_TX_WAIT;
            break;
        case CANTP_CORE_TX_FF_WAIT:
            nodePtr->status = CANTP_CORE_RX_FC;
            nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBs;
            /* TODO: after update fifo */
            break;
        case CANTP_CORE_TX_CF_WAIT:
        {
            if (nodePtr->bytesNums == nodePtr->allBytesNum)
            {
                /* all data is sended ok */
                CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_OK);
                nodePtr->status = CANTP_CORE_TX_WAIT;
            }
            else
            {
                /* have data to send */
                if (nodePtr->BSCnt == 0U && nodePtr->ctsFlag == CANTP_FALSE)
                {
                    nodePtr->status = CANTP_CORE_RX_FC;
                    nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nBs;
                }
                else
                {
                    nodePtr->status = CANTP_CORE_TX_WAIT_CS;
                    cantp_u16 waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nCs;
                    if (nodePtr->STmin > waitTime)
                    {
                        waitTime = nodePtr->STmin;
                    }
                    nodePtr->waitTime = waitTime;
                }

            }
            break;
        }
        default:
            break;
    }
}

void CanTp_Core_TxWaitTimeout(cantp_core_task_node_type_t* nodePtr)
{
    switch (nodePtr->status)
    {
        case CANTP_CORE_TX_SF:
        case CANTP_CORE_TX_SF_WAIT:
        case CANTP_CORE_TX_FF:
        case CANTP_CORE_TX_FF_WAIT:
        case CANTP_CORE_TX_CF:
        case CANTP_CORE_TX_CF_WAIT:
            CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_TIMEOUT_A);
            nodePtr->status = CANTP_CORE_TX_WAIT;
            break;
        case CANTP_CORE_TX_WAIT_CS:
            nodePtr->status = CANTP_CORE_TX_CF;
            nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nAs;
            break;

        case CANTP_CORE_RX_FC:
            CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_TIMEOUT_Bs);
            nodePtr->status = CANTP_CORE_TX_WAIT;
            break;
        default:
            CanTp_TxConfirmation(nodePtr->sduId, CANTP_CORE_N_ERROR);
            nodePtr->status = CANTP_CORE_TX_WAIT;
            break;
        }

}

void CanTp_Core_RxWaitTimeout(cantp_core_task_node_type_t* nodePtr)
{
    switch (nodePtr->status)
    {
        case CANTP_CORE_RX_CF:
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_TIMEOUT_Cr);
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        case CANTP_CORE_TX_FC:
        case CANTP_CORE_TX_FC_WAIT:
        case CANTP_CORE_RX_ABORT:
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_TIMEOUT_A);
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
        case CANTP_CORE_RX_WAIT_BR:
            nodePtr->status = CANTP_CORE_TX_FC;
            nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nAr;
            // CanTp_Core_TxFC(nodePtr->sduId);
            break;
        case CANTP_CORE_RX_ABORT_WAIT:
            nodePtr->status = CANTP_CORE_RX_ABORT;
            nodePtr->waitTime = CanTp_Core_timeArrayPtr[nodePtr->sduId].nAr;
            break;
        default:
            CanTp_RxIndication(nodePtr->sduId, CANTP_CORE_N_ERROR);
            nodePtr->status = CANTP_CORE_RX_WAIT;
            break;
    }
}

void CanTp_Core_MainFunction(void)
{
    cantp_core_task_node_type_t* nodePtr;
    for (cantp_u8 idx = 0U; idx < CANTP_CHANNEL_NUM; idx++)
    {
        nodePtr = &CanTp_Core_txNodeArray[idx];
        if (nodePtr->status != CANTP_CORE_TX_WAIT && nodePtr->waitTime == 0)
        {
            CanTp_Core_TxWaitTimeout(nodePtr);
        }

        nodePtr = &CanTp_Core_rxNodeArray[idx];
        if (nodePtr->status != CANTP_CORE_RX_WAIT && nodePtr->waitTime == 0)
        {
            CanTp_Core_RxWaitTimeout(nodePtr);
        }

    }
    
    for (cantp_u8 idx = 0U; idx < CANTP_CHANNEL_NUM; idx++)
    {
        nodePtr = &CanTp_Core_rxNodeArray[idx];
        if (nodePtr->status == CANTP_CORE_TX_FC || nodePtr->status == CANTP_CORE_RX_ABORT)
        {
            CanTp_Core_TxFC(idx);
        }
    }
    
}

void CanTp_Core_RxIndication(pdu_id_type rxSduId, const pdu_info_type_t* pduInfoPtr)
{
    do {
        cantp_u8 pciOffset = CanTp_Core_pciOffsetArray[rxSduId];
        /* No pci unit in this msg */
        if (pduInfoPtr->sduLength < pciOffset + 1)
        {
            break;
        }
        cantp_u8* pciPtr = pduInfoPtr->sduDataPtr + pciOffset;

        /* use additional data bytes, but data bytes not equal */
        if ((pciOffset != 0U) && (pduInfoPtr->sduDataPtr[0] != CanTp_Core_channelArrayPtr[rxSduId].compareByte))
        {
            break;
        }
        pdu_info_type_t tempInfo = {
            .metaDataPtr = pduInfoPtr->metaDataPtr,
            .sduDataPtr = pciPtr,
            .sduLength = pduInfoPtr->sduLength - pciOffset
        };
        switch (pciPtr[0] >> 4U)
        {
            case CANTP_CORE_SF:
                /* Abort current node to restart RX_SF */
                if (CanTp_Core_rxNodeArray[rxSduId].status != CANTP_CORE_RX_WAIT)
                {
                    CanTp_RxIndication(rxSduId, CANTP_CORE_N_UNEXP_PDU);
                    CanTp_Core_rxNodeArray[rxSduId].status = CANTP_CORE_RX_WAIT;
                }
                CanTp_Core_RxSF(&CanTp_Core_rxNodeArray[rxSduId], &tempInfo);
                break;
            case CANTP_CORE_FF:
                /* Abort current node to restart RX_FF */
                if (CanTp_Core_rxNodeArray[rxSduId].status != CANTP_CORE_RX_WAIT)
                {
                    CanTp_RxIndication(rxSduId, CANTP_CORE_N_UNEXP_PDU);
                    CanTp_Core_rxNodeArray[rxSduId].status = CANTP_CORE_RX_WAIT;
                }
                CanTp_Core_RxFF(&CanTp_Core_rxNodeArray[rxSduId], &tempInfo);
                break;
            case CANTP_CORE_CF:
                if (CanTp_Core_rxNodeArray[rxSduId].status == CANTP_CORE_RX_CF)
                {
                    /* Ignore CF in other status */
                    CanTp_Core_RxCF(&CanTp_Core_rxNodeArray[rxSduId], &tempInfo);
                }
                break;
            case CANTP_CORE_FC:
                CanTp_Core_RxFC(&CanTp_Core_txNodeArray[rxSduId], &tempInfo);
                break;
            default:
                /* Ignore unknown pdu and FC */
                break;
        }
    } while (CANTP_FALSE);
}

void CanTp_Core_TxConfirmation(pdu_id_type txPduId, cantp_return_type result)
{
    pdu_id_type sduId = txPduId / 2;
    if (txPduId % 2)
    {
        CanTp_Core_ConfirmFc(&CanTp_Core_rxNodeArray[sduId]);
    }
    else
    {
        CanTp_Core_ConfirmTx(&CanTp_Core_txNodeArray[sduId]);
    }
}

void CanTp_Core_DecreaseCounter()
{
    for (cantp_u8 idx = 0; idx < CANTP_CHANNEL_NUM; idx++)
    {
        if (CanTp_Core_txNodeArray[idx].status != CANTP_CORE_TX_WAIT && CanTp_Core_txNodeArray[idx].waitTime)
        {
            CanTp_Core_txNodeArray[idx].waitTime--;
        }
        if (CanTp_Core_rxNodeArray[idx].status != CANTP_CORE_RX_WAIT && CanTp_Core_rxNodeArray[idx].waitTime)
        {
            CanTp_Core_rxNodeArray[idx].waitTime--;
        }
    }
}
