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


#define LOG_SIGN "CAN"
#define LOGGER &env->slog->logger

_Noreturn void CanTpProcessing_ListenerTask0(tCanSerialPortFrameTp *env);

_Noreturn void CanTpProcessing_ListenerTask1(tCanSerialPortFrameTp *env);

void CanSerialPortFrameTpInit(
        tCanSerialPortFrameTp *env,

        uint8_t numBuf,

        SystemMutexCmsis txAccessQueue,

        tReceivedCan_func receivedCan_func,
        void *callback_argCan,

        tReceivedTP_func receivedTP_func,
        void *callback_arg,

        tSerialPortFrameIO *ioCanFrame,
        tCanTP_data *dataBuf,
        uint16_t dataBufMaxSize,
        tLoggerToSerialPort *slog,

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

) {
    env->ioCanFrame = ioCanFrame;

    env->numBuf = numBuf;

    env->txAccessQueue = txAccessQueue;

    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->slog = slog;

    InitThreadBlock(env->T_can_Listener0, "CanListener0", osPriorityNormal);
    InitThreadBlock(env->T_can_Listener1, "CanListener1", osPriorityNormal);
}

void CanSerialPortFrameTp_Start(tCanSerialPortFrameTp *env, uint8_t numBuf) {
    if (numBuf == 0)
        ThreadBlock_Start(env->T_can_Listener0, env, CanTpProcessing_ListenerTask0);

    if (numBuf == 1)
        ThreadBlock_Start(env->T_can_Listener1, env, CanTpProcessing_ListenerTask1);
}

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

    LoggerStrInfo(LOGGER, LOG_SIGN, env->hexString, strlen(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 = 0;

    if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {

        CanSerialPortFrameSetId(env->ioCanFrame->env, id);

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

        osMutexRelease(env->txAccessQueue);
    } else {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendFlowFrame")
    }

    return sent;
}

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;

    if (env->numBuf == 0) {
        recv = env->ioCanFrame->receive0(env->ioCanFrame->env, (uint8_t *) &canFrame, 1, timeout);
    }

    if (env->numBuf == 1) {
        recv = env->ioCanFrame->receive1(env->ioCanFrame->env, (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_ListenerTask0(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");
            } else {
                LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Принят не ожидаемый Flow control Frame");
            }
        }

        if (result == TP_RECEIVED_DATA) {
//            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Приняты данные: ");
//            sendLogCanHex(env, env->dataBuf->data, env->dataBuf->len);

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

        }

        if (result == TP_ERROR_FS_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (1)");
        }

        if (result == TP_ERROR_FC_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Выставлен статус ошибки в Flow control Frame");
        }

        if (result == TP_ERROR_FF_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN,
                                "0: Ошибка. Принят First Frame FF, но еще не завершен прием Consecutive Frame");
        }


        if (result == TP_ERROR_FF_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (2)");
        }

        if (result == TP_ERROR_CF_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Принят не ожидаемый Consecutive Frame");
        }

        if (result == TP_ERROR_CF_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Переполнение приемного буфера (3)");
        }

        if (result == TP_ERROR_CF_ERROR_SN) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Ошибка. Не совпадение счетчика SN в Consecutive Frame");
        }

        if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "0: Истекло время ожидания приема Consecutive Frame");
        }
    }
}

_Noreturn void CanTpProcessing_ListenerTask1(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");
            } else {
                LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Принят не ожидаемый Flow control Frame");
            }
        }

        if (result == TP_RECEIVED_DATA) {
//            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "Приняты данные: ");
//            sendLogCanHex(env, env->dataBuf->data, env->dataBuf->len);

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

        }

        if (result == TP_ERROR_FS_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (1)");
        }

        if (result == TP_ERROR_FC_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Выставлен статус ошибки в Flow control Frame");
        }

        if (result == TP_ERROR_FF_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN,
                                "1: Ошибка. Принят First Frame FF, но еще не завершен прием Consecutive Frame");
        }


        if (result == TP_ERROR_FF_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (2)");
        }

        if (result == TP_ERROR_CF_ERROR) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Принят не ожидаемый Consecutive Frame");
        }

        if (result == TP_ERROR_CF_ERROR_SIZE) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Переполнение приемного буфера (3)");
        }

        if (result == TP_ERROR_CF_ERROR_SN) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Ошибка. Не совпадение счетчика SN в Consecutive Frame");
        }

        if (result == TP_ERROR_FF_ERROR_TIMEOUT) {
            LoggerStrInfoStatic(LOGGER, LOG_SIGN, "1: Истекло время ожидания приема Consecutive Frame");
        }
    }
}


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 = 0;

    if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {

        CanSerialPortFrameSetId(env->ioCanFrame->env, id);

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

        osMutexRelease(env->txAccessQueue);
    } else {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendSingleFrame")
    }

    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 = 0;

    if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {

        CanSerialPortFrameSetId(env->ioCanFrame->env, id);

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

        osMutexRelease(env->txAccessQueue);
    } else {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendConsecutiveFrame")
    }

    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 = 0;

    if (osMutexAcquire(env->txAccessQueue, 1000) == osOK) {

        CanSerialPortFrameSetId(env->ioCanFrame->env, id);

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

        osMutexRelease(env->txAccessQueue);
    } else {
        LoggerInfoStatic(LOGGER, LOG_SIGN, "Ошибка доступа sendFirstFrame")
    }

    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;
}