//
// Created by cfif on 16.03.2024.
//
#include <SystemDelayInterface.h>
#include "CanSerialPortFrameTP.h"
#include "memory.h"
#include "AsciiStringAssmeblingUtils.h"
#include "SystemMutexCmsis.h"
#include "CanPorts.h"
#include <stdio.h>

#define LOG_SIGN "CAN_TP"
#define LOGGER env->logger


static char strPrintfDebug_TP[80];

static void PrintfDebug(uint8_t *data, uint8_t dlc) {

    switch (dlc) {

        case 0:
            sprintf(strPrintfDebug_TP, " ");
            break;
        case 1:
            sprintf(strPrintfDebug_TP, "%02X", data[0]);
            break;
        case 2:
            sprintf(strPrintfDebug_TP, "%02X:%02X", data[0], data[1]);
            break;
        case 3:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X", data[0], data[1], data[2]);
            break;
        case 4:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3]);
            break;
        case 5:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3],
                    data[4]);
            break;
        case 6:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2], data[3],
                    data[4], data[5]);
            break;
        case 7:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X:%02X", data[0], data[1], data[2],
                    data[3], data[4],
                    data[5], data[6]);
            break;
        case 8:
            sprintf(strPrintfDebug_TP, "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X ", data[0], data[1],
                    data[2], data[3], data[4],
                    data[5], data[6], data[7]);
            break;
        default:
            sprintf(strPrintfDebug_TP, " ");
    }

}


char *sendLogCanTpHex(tCanSerialPortFrameTp *env, uint8_t *data, size_t size) {
    memset(env->hexString, 0, sizeof(env->hexString));
    size_t len = 0;


    uint8_t full = size / 8;
    uint8_t tail = size % 8;

    for (uint8_t i = 0; i < full; ++i) {
        PrintfDebug(&data[i * 8], 8);

        if ((len + strlen(strPrintfDebug_TP))  < LEN_DEBUG_TP_BUFF) {
            len += strlen(strPrintfDebug_TP);
            memcpy(env->hexString, strPrintfDebug_TP, strlen(strPrintfDebug_TP));
        } else {
            return env->hexString;
        }

    }

    if (tail > 0) {
        PrintfDebug(&data[full * 8], tail);

        if ((len + strlen(strPrintfDebug_TP))  < LEN_DEBUG_TP_BUFF) {
            len += strlen(strPrintfDebug_TP);
            memcpy(env->hexString, strPrintfDebug_TP, strlen(strPrintfDebug_TP));
        } else {
            return env->hexString;
        }

    }

    return env->hexString;
}

uint16_t sendFlowFrame(tCanSerialPortFrameTp *env, uint32_t id, uint32_t timeout) {
    uint8_t dataTpFrame[8];
    memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));

    eTpFrameFC *frame = (eTpFrameFC *) dataTpFrame;
    frame->typeFrame = TP_TYPE_FRAME_FC;
    frame->status = 0;
    frame->blockSize = 0;
    frame->timeST = 0;

    uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
                                              PROTOCOL_CAN_TYPE_UDS, timeout);

    return sent;
}


uint16_t sendSingleFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
    uint8_t dataTpFrame[8];
    memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));

    eTpFrameSF *frame = (eTpFrameSF *) dataTpFrame;
    frame->typeFrame = TP_TYPE_FRAME_SF;
    frame->dl = size;

    memcpy(&frame->data[0], data, size);

    uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
                                              PROTOCOL_CAN_TYPE_UDS, timeout);

    return sent;
}

uint16_t sendConsecutiveFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint8_t sn,
                              uint32_t timeout) {
    uint8_t dataTpFrame[8];
    memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));

    eTpFrameCF *frame = (eTpFrameCF *) dataTpFrame;
    frame->typeFrame = TP_TYPE_FRAME_CF;
    frame->sn = sn;

    memcpy(&frame->data[0], data, size);


    uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
                                              PROTOCOL_CAN_TYPE_UDS, timeout);


    return sent;

}

bool waitFlowControl(tCanSerialPortFrameTp *env) {
    env->isFlowControl = true;

    uint32_t timeEnd = SystemGetMs() + WAIT_FC_FRAME_TIMEOUT;

    while (timeEnd > SystemGetMs()) {
        if (!env->isFlowControl) {
            break;
        }
    }

    if (env->isFlowControl) {
        LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Таймаут ожидания Flow control Frame");
        return false;
    }

    return true;
}

bool sendFirstFrame(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
    uint8_t dataTpFrame[8];
    uint16_t pDataLen = 0;

    memset(dataTpFrame, TP_FRAME_PADDING, sizeof(dataTpFrame));

    eTpFrameFF *frame = (eTpFrameFF *) dataTpFrame;
    frame->typeFrame = TP_TYPE_FRAME_FF;
    frame->dlL = size;
    frame->dlM = (size >> 8);

    memcpy(&frame->data[0], &data[pDataLen], 6);
    pDataLen += 6;

    uint16_t sent = env->ioCanFrame->transmit(env->ioCanFrame->env, (uint8_t *) frame, 8, id,
                                              PROTOCOL_CAN_TYPE_UDS, timeout);

    bool result = waitFlowControl(env);
    if (!result)
        return false;

    uint16_t countPacketFull = (size - 6) / 7;
    uint16_t sizePacketTail = (size - 6) % 7;

    uint8_t sn = 1;

    for (uint16_t i = 0; i < countPacketFull; ++i) {

        sendConsecutiveFrame(env, &data[pDataLen], 7, id, sn, timeout);
        pDataLen += 7;

        ++sn;
        if (sn == 15)
            sn = 1;
    }

    if (sizePacketTail) {
        sendConsecutiveFrame(env, &data[pDataLen], sizePacketTail, id, sn, timeout);
        pDataLen += sizePacketTail;
    }

    return true;

}

bool
CanSerialPortFrameTpTransmit(tCanSerialPortFrameTp *env, uint8_t *data, uint16_t size, uint32_t id, uint32_t timeout) {
    bool result;

    // Single Frame SF
    if (size <= 7) {
        result = sendSingleFrame(env, data, size, id, timeout);
    } else {
        result = sendFirstFrame(env, data, size, id, timeout);
    }

    return result;
}

eTpResult vCanSerialPortFrameTpReceive(tCanSerialPortFrameTp *env, uint32_t timeout) {
    can_rx_message_type canFrame;

    if ((env->timeoutFF > 0) && (env->timeoutFF < SystemGetMs())) {
        env->timeoutFF = 0;
        env->dataReceivedConsecutiveFrameLen = 0;
        env->dataBufLen = 0;
        env->dataBufWorkLen = 0;

        return TP_ERROR_FF_ERROR_TIMEOUT;
    }

    uint16_t recv = 0;

    recv = env->ioCanFrame->receive(env->ioCanFrame->env, PROTOCOL_CAN_UDS, (uint8_t *) &canFrame, 1, timeout);

    if (recv == 0)
        return TP_STANDBY;

    bool isTesterPresent = env->ReceivedCan_func(env->callback_argCan, &canFrame);

    if (isTesterPresent) {
        return TP_STANDBY;
    }

    bool isFilterFind = false;
    uint32_t reqId = 0;
    for (uint8_t i = 0; i < env->filterIdCount; ++i) {
        if (canFrame.standard_id == env->filterReqId[i]) {

            reqId = env->filterRespId[i];

            isFilterFind = true;
            break;
        }
    }

    if (!isFilterFind) {
        return TP_ERROR_WRONG_ADDRESS;
    }


    if ((recv > 0) && (canFrame.dlc == 8)) {

        // Flow control Frame – FC – фрейм управления потоком
        if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_FC) {
            eTpFrameFC *frame = (eTpFrameFC *) canFrame.data;

            if (frame->status == 0)
                return TP_RECEIVED_FC_DATA;

            return TP_ERROR_FC_ERROR;
        }

        // Single Frame SF – Однократный фрейм
        if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_SF) {
            eTpFrameSF *frame = (eTpFrameSF *) canFrame.data;

            if (frame->dl < env->dataBufMaxSize) {
                //memcpy(env->dataBuf, &canFrame.data[1], frame->dl);
                env->dataBufLen = frame->dl;

                memcpy(env->dataBuf->data, &canFrame.data[1], frame->dl);
                env->dataBuf->len = env->dataBufLen;
                env->dataBuf->adrCan = canFrame.standard_id;

                env->dataBufWorkLen = 0;
                return TP_RECEIVED_DATA;
            } else {
                env->dataBufWorkLen = 0;
                return TP_ERROR_FS_ERROR_SIZE;
            }
        }

        // First Frame FF – Первый фрейм из серии фреймов
        if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_FF) {
            if (env->dataReceivedConsecutiveFrameLen) {
                return TP_ERROR_FF_ERROR;
            }

            eTpFrameFF *frame = (eTpFrameFF *) canFrame.data;

            if (env->dataBufWorkLen + (frame->dlL | (frame->dlM << 8)) > env->dataBufMaxSize) {
                return TP_ERROR_FF_ERROR_SIZE;
            }

            uint16_t dll = frame->dlL | (frame->dlM << 8);
            if (dll > 100) {
                asm("nop");
            }

            env->timeoutFF = SystemGetMs() + WAIT_FF_FRAME_TIMEOUT;

            env->sn = 1;
            env->dataBufWorkLen = 0;
            env->dataReceivedConsecutiveFrameLen = (frame->dlL | (frame->dlM << 8));

            for (uint8_t j = 2; j < 8; ++j) {
                //env->dataBuf[env->dataBufWorkLen] = canFrame.data[j];
                env->dataBuf->data[env->dataBufWorkLen] = canFrame.data[j];

                ++env->dataBufWorkLen;
                if (env->dataBufWorkLen > env->dataBufMaxSize) {
                    env->timeoutFF = 0;
                    env->dataReceivedConsecutiveFrameLen = 0;
                    env->dataBufWorkLen = 0;
                    return TP_ERROR_CF_ERROR_SIZE;
                }

                --env->dataReceivedConsecutiveFrameLen;
            }

            sendFlowFrame(env, reqId, WAIT_FRAME_WRITE);
        }

        // Consecutive Frame  CF – последующий фрейм.
        if ((canFrame.data[0] >> 4) == TP_TYPE_FRAME_CF) {

            if (env->dataReceivedConsecutiveFrameLen == 0) {
                return TP_ERROR_CF_ERROR;
            }

            eTpFrameCF *frame = (eTpFrameCF *) canFrame.data;

            if (frame->sn != env->sn) {
                env->timeoutFF = 0;
                env->dataReceivedConsecutiveFrameLen = 0;
                env->dataBufWorkLen = 0;
                return TP_ERROR_CF_ERROR_SN;
            }

            ++env->sn;
            if (env->sn > 15)
                env->sn = 0;

            for (uint8_t j = 1; j < 8; ++j) {
                //env->dataBuf[env->dataBufWorkLen] = canFrame.data[j];
                env->dataBuf->data[env->dataBufWorkLen] = canFrame.data[j];

                ++env->dataBufWorkLen;
                if (env->dataBufWorkLen > env->dataBufMaxSize) {
                    env->timeoutFF = 0;
                    env->dataReceivedConsecutiveFrameLen = 0;
                    env->dataBufWorkLen = 0;
                    return TP_ERROR_CF_ERROR_SIZE;
                }

                --env->dataReceivedConsecutiveFrameLen;
                if (env->dataReceivedConsecutiveFrameLen == 0) {
                    env->timeoutFF = 0;
                    env->dataBufLen = env->dataBufWorkLen;

                    env->dataBuf->len = env->dataBufLen;
                    env->dataBuf->adrCan = canFrame.standard_id;

                    env->dataBufWorkLen = 0;
                    return TP_RECEIVED_DATA;
                }
            }

        }

    }

    return TP_ERROR_FC_TIMEOUT;
}


_Noreturn void CanTpProcessing_ListenerTask(tCanSerialPortFrameTp *env) {
    while (1) {
        eTpResult result = vCanSerialPortFrameTpReceive(env, 1000);

        if (result == TP_RECEIVED_FC_DATA) {
            if (env->isFlowControl) {
                env->isFlowControl = false;
                LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Flow control frame accepted");
            } else {
                LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: An unexpected Flow Control Frame was received");
            }
        }

        if (result == TP_RECEIVED_DATA) {
            //sendLogCanTpHex(env, env->dataBuf->data, env->dataBuf->len);
            //LoggerFormatInfo(LOGGER, LOG_SIGN, "Data received: %s", env->hexString)

            env->receivedTP_func(env->callback_argTp, env->dataBuf);

        }

        if (result == TP_ERROR_FS_ERROR_SIZE) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (1)");
        }

        if (result == TP_ERROR_FC_ERROR) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: An error status was set in the Flow Control Frame");
        }

        if (result == TP_ERROR_FF_ERROR) {
            LoggerErrorStatic(LOGGER, LOG_SIGN,
                              "Error: First Frame FF received, but Consecutive Frame reception is not yet complete");
        }


        if (result == TP_ERROR_FF_ERROR_SIZE) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (2)");
        }

        if (result == TP_ERROR_CF_ERROR) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Unexpected Consecutive Frame received");
        }

        if (result == TP_ERROR_CF_ERROR_SIZE) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: Receive buffer overflow (3)");
        }

        if (result == TP_ERROR_CF_ERROR_SN) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: SN counter mismatch in Consecutive Frame");
        }

        if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
            LoggerErrorStatic(LOGGER, LOG_SIGN, "Error: The Consecutive Frame timed out");
        }
    }
}

void CanSerialPortFrameTp_Start(tCanSerialPortFrameTp *env) {
    ThreadBlock_Start(env->T_can_Listener, env, CanTpProcessing_ListenerTask);
}

void CanSerialPortFrameTpInit(
        tCanSerialPortFrameTp *env,

        tReceivedCan_func receivedCan_func,
        void *callback_argCan,

        tReceivedTP_func receivedTP_func,
        void *callback_arg,

        tSerialPortFrameIO *ioCanFrame,
        tCanTP_data *dataBuf,
        uint16_t dataBufMaxSize,
        tLoggerInterface *logger,

        uint8_t filterIdCount,
        uint32_t *filterReqId,
        uint32_t *filterRespId,
        uint8_t *filterDirReq

) {
    env->ioCanFrame = ioCanFrame;

    env->dataBuf = dataBuf;
    env->dataBufMaxSize = dataBufMaxSize;
    env->dataBufLen = 0;
    env->dataBufWorkLen = 0;
    env->isFlowControl = false;

    env->filterIdCount = filterIdCount;
    env->filterReqId = filterReqId;
    env->filterRespId = filterRespId;
    env->filterDirReq = filterDirReq;

    env->dataReceivedConsecutiveFrameLen = 0;


    env->ReceivedCan_func = receivedCan_func;
    env->callback_argCan = callback_argCan;


    env->receivedTP_func = receivedTP_func;
    env->callback_argTp = callback_arg;

    env->timeoutFF = 0;

    env->logger = logger;

    InitThreadBlock(env->T_can_Listener, "CanTpUds", osPriorityNormal);
}