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

#define LOG_SIGN "CAN_XCP"
#define LOGGER &env->logger

static unsigned long SeedToKeyHVAC_NAMI(unsigned char rnd, unsigned long dwSeedInfo) {
#define Mask 0x55F388A6UL

    unsigned char i;
    unsigned long key = dwSeedInfo;

    rnd += 35;
    if (rnd < 35) rnd = 255;

    for (i = 0; i < rnd; i++) {
        if (key & 0x80000000UL)
            key = (key << 1) ^ Mask;
        else
            key <<= 1;
    }

    return key;
}

void CanSerialPortFrameXcpInit(
        tCanSerialPortFrameXCP *env,
        tSerialPortFrameIO *ioCanFrame,
        tLoggerInterface *logger
) {
    env->ioCanFrame = ioCanFrame;


    env->logger = logger;

    InitThreadBlock(env->T_can_Listener_XCP, "CanListenerXCP", osPriorityNormal);
}

eXcpResult vCanSerialPortFrameXcpReceive(tCanSerialPortFrameXCP *env, uint32 timeout) {

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

    if (recv == 0)
        return XCP_STANDBY;

    env->isTransmitResponse = true;
    env->isTransmitResponseError = false;

    eXcpTypeCommand com = env->canFrame.data[0];

    switch (com) {
        case XCP_COMMAND_CONNECT: {
            eXcpCommand_CONNECT *xcpCommand_CONNECT = (eXcpCommand_CONNECT *) env->response;

            xcpCommand_CONNECT->COM = XCP_COMMAND_CONNECT;

            xcpCommand_CONNECT->CAL_PAG = 0; // Калибровка и пагинация
            xcpCommand_CONNECT->reserve1 = 0;
            xcpCommand_CONNECT->DAQ = 1; // Поддержка DAQ
            xcpCommand_CONNECT->STIM = 1; // Стимуляция (формирование инициативных запросов)
            xcpCommand_CONNECT->PGM = 1; // Программирование FLASH
            xcpCommand_CONNECT->reserve2 = 0;
            xcpCommand_CONNECT->reserve3 = 0;
            xcpCommand_CONNECT->reserve4 = 0;

            xcpCommand_CONNECT->BYTE__ORDER = 0; // 0 - Intel 1 - Motorola
            xcpCommand_CONNECT->ADDRESS_GRANULARITY_0 = 0;
            xcpCommand_CONNECT->ADDRESS_GRANULARITY_1 = 0;
            xcpCommand_CONNECT->reserve5 = 1;
            xcpCommand_CONNECT->reserve6 = 1;
            xcpCommand_CONNECT->reserve7 = 1;
            xcpCommand_CONNECT->SLAVE_BLOCK_MODE = 0;
            xcpCommand_CONNECT->OPTIONAL = 1; // Поддержка команды XCP_COMMAND_GET_STATUS

            xcpCommand_CONNECT->MAX_CTO_ = MAX_CTO;
            xcpCommand_CONNECT->MAX_DTO_ = MAX_DTO;

            xcpCommand_CONNECT->XCP_PROTOCOL_VER = 1;
            xcpCommand_CONNECT->XCP_TRANSPORT_VER = 1;

            env->responseSize = 8;

            break;
        }

        case XCP_COMMAND_DISCONNECT: {
            env->isTransmitResponse = false;
            break;
        };

        case XCP_COMMAND_GET_STATUS: {
            eXcpCommand_GET_STATUS *xcpCommand_GET_STATUS = (eXcpCommand_GET_STATUS *) env->response;

            xcpCommand_GET_STATUS->COM = XCP_COMMAND_CONNECT;

            xcpCommand_GET_STATUS->STORE_CAL_REQ = 0;
            xcpCommand_GET_STATUS->reserve1 = 0;
            xcpCommand_GET_STATUS->STORE_DAQ_REQ = 0;
            xcpCommand_GET_STATUS->CLEAR_DAQ_REQ = 0;
            xcpCommand_GET_STATUS->reserve2 = 0;
            xcpCommand_GET_STATUS->reserve3 = 0;
            xcpCommand_GET_STATUS->DAQ_RUNNING = 0;
            xcpCommand_GET_STATUS->RESUME = 0;

            xcpCommand_GET_STATUS->CAL_PAG = 0;
            xcpCommand_GET_STATUS->reserve5 = 0;
            xcpCommand_GET_STATUS->DAQ = 0;
            xcpCommand_GET_STATUS->STIM = 0;
            xcpCommand_GET_STATUS->PGM = 0;
            xcpCommand_GET_STATUS->reserve6 = 0;
            xcpCommand_GET_STATUS->reserve7 = 0;
            xcpCommand_GET_STATUS->reserve8 = 0;

            xcpCommand_GET_STATUS->STATE_NUMBER = 0;

            xcpCommand_GET_STATUS->SESSION_CONFIGURATION_ID = 0;

            env->responseSize = 8;

            break;
        };

        case XCP_COMMAND_SYNCH: {
            env->response[0] = XCP_COMMAND_DISCONNECT;
            env->response[1] = XCP_ERR_CMD_SYNCH;

            env->responseSize = 2;

            break;
        };


        case XCP_GET_COMM_MODE_INFO: {
            eXcpCommand_GET_COMM_MODE_INFO *xcpCommand_GET_COMM_MODE_INFO = (eXcpCommand_GET_COMM_MODE_INFO *) env->response;

            xcpCommand_GET_COMM_MODE_INFO->RESERVED1 = 0;

            xcpCommand_GET_COMM_MODE_INFO->MASTER_BLOCK_MODE = 1;
            xcpCommand_GET_COMM_MODE_INFO->INTERLEAVED_MODE = 0;
            xcpCommand_GET_COMM_MODE_INFO->RESERVED2 = 0;

            xcpCommand_GET_COMM_MODE_INFO->RESERVED3 = 0;
            xcpCommand_GET_COMM_MODE_INFO->MAX_BS = 255;
            xcpCommand_GET_COMM_MODE_INFO->MIN_ST = 0;
            xcpCommand_GET_COMM_MODE_INFO->QUEUE_SIZE = 0;
            xcpCommand_GET_COMM_MODE_INFO->VERSION = 1;

            env->responseSize = 8;
            break;
        }

        case XCP_GET_SEED: {
            eXcpCommand_REQ_GET_SEED *request = (eXcpCommand_REQ_GET_SEED *) env->canFrame.data;

            if (request->MODE == 0) {

                eXcpCommand_GET_SEED *xcpCommand_GET_SEED = (eXcpCommand_GET_SEED *) env->response;

                xcpCommand_GET_SEED->COM = XCP_COMMAND_CONNECT;
                xcpCommand_GET_SEED->LEN = 4;
                xcpCommand_GET_SEED->KEY = 0x11223344;

                env->RND = request->RESOURCE;

                env->responseSize = 6;
            }

            break;
        }

        case XCP_UNLOCK: {

            eXcpCommand_REQ_UNLOCK *request = (eXcpCommand_REQ_UNLOCK *) env->canFrame.data;

            uint32_t securitySeedMy = SeedToKeyHVAC_NAMI(env->RND, 0x11223344);

            if (securitySeedMy != request->KEY) {
                env->response[0] = XCP_COMMAND_DISCONNECT;
                env->response[1] = XCP_ERR_ACCESS_LOCKED;
                env->responseSize = 2;
            } else {
                env->response[0] = XCP_COMMAND_CONNECT;
                env->response[1] = 0;
                env->responseSize = 2;
            }

            break;
        }

        case XCP_COMMAND_SET_MTA: {
            eXcpCommand_SET_MTA *request = (eXcpCommand_SET_MTA *) env->canFrame.data;

            env->ADR_EXT = request->ADR_EX;
            env->ADR_MTA = request->ADR;

            env->response[0] = XCP_COMMAND_CONNECT;
            env->responseSize = 1;

            break;
        };

        case XCP_COMMAND_UPLOAD: {
            eXcpCommand_Upload *request = (eXcpCommand_Upload *) env->canFrame.data;

            env->isTransmitResponse = false;

            env->response[0] = XCP_COMMAND_CONNECT;

            uint8_t fullBlock = request->COUNT / 7;
            uint8_t tailBlock = request->COUNT % 7;

            for (uint8_t i = 0; i < fullBlock; ++i) {
                memcpy(&env->response[1], (uint8_t *) (env->ADR_MTA), request->COUNT);
                env->ADR_MTA += 7;
                env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, 7, timeout);
            }

            if (tailBlock > 0) {
                memcpy(&env->response[1], (uint8_t *) (env->ADR_MTA), tailBlock);
                env->ADR_MTA += tailBlock;
                env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, tailBlock, timeout);
            }

            break;
        };

        case XCP_COMMAND_SHORT_UPLOAD: {
            eXcpCommand_SHORT_UPLOAD *request = (eXcpCommand_SHORT_UPLOAD *) env->canFrame.data;

            if (request->COUNT > (MAX_CTO - 1)) {
                env->response[0] = XCP_COMMAND_DISCONNECT;
                env->response[1] = XCP_ERR_OUT_OF_RANGE;
                break;
            }

            env->response[0] = XCP_COMMAND_CONNECT;

            memcpy(&env->response[1], (uint8_t *) request->ADR, request->COUNT);

            env->responseSize = request->COUNT + 1;
            break;
        };


        case XCP_COMMAND_DOWNLOAD: {
            eXcpCommand_Download *request = (eXcpCommand_Download *) env->canFrame.data;

            uint8_t COUNT = env->canFrame.dlc - 2;

            memcpy((uint8_t *) (env->ADR_MTA), &env->response[2], COUNT);
            env->ADR_MTA += COUNT;

            env->response[0] = XCP_COMMAND_CONNECT;
            env->responseSize = 1;

            env->downloadRemaining = request->COUNT - COUNT;

            break;
        };

        case XCP_COMMAND_DOWNLOAD_NEXT: {
            eXcpCommand_Download *request = (eXcpCommand_Download *) env->canFrame.data;

            uint8_t COUNT = env->canFrame.dlc - 2;

            memcpy((uint8_t *) (env->ADR_MTA), &env->response[2], COUNT);
            env->ADR_MTA += COUNT;

            if (request->COUNT != env->downloadRemaining) {
                env->response[0] = XCP_COMMAND_DISCONNECT;
                env->response[1] = XCP_ERR_SEQUENCE;
                break;
            }

            env->downloadRemaining -= COUNT;

            env->isTransmitResponse = false;

            if (env->downloadRemaining == 0) {
                env->isTransmitResponse = true;
                env->response[0] = XCP_COMMAND_CONNECT;
                env->responseSize = 1;
            }

            break;
        };

        case XCP_COMMAND_DOWNLOAD_MAX: {
            uint8_t COUNT = env->canFrame.dlc - 1;

            memcpy((uint8_t *) (env->ADR_MTA), &env->response[1], COUNT);
            env->ADR_MTA += COUNT;

            env->response[0] = XCP_COMMAND_CONNECT;
            env->responseSize = 1;

            break;
        };

        case XCP_CLEAR_DAQ_LIST: {

            break;
        }

        case XCP_GET_DAQ_LIST_INFO: {
            eXcpCommand_Get_DAQ_List_Info *xcpCommand_Get_DAQ_List_Info = (eXcpCommand_Get_DAQ_List_Info *) env->response;

            xcpCommand_Get_DAQ_List_Info->COM = XCP_COMMAND_CONNECT;

            xcpCommand_Get_DAQ_List_Info-> PREDEFINED = ;
            xcpCommand_Get_DAQ_List_Info-> EVENT_FIXED = ;
            xcpCommand_Get_DAQ_List_Info-> DAQ = ;
            xcpCommand_Get_DAQ_List_Info-> STIM = ;
            xcpCommand_Get_DAQ_List_Info-> RESERVER = 0;

            xcpCommand_Get_DAQ_List_Info->MAX_ODT_ = MAX_ODT;
            xcpCommand_Get_DAQ_List_Info->MAX_ODT_ENTRIES_ = MAX_ODT_ENTRIES;
            xcpCommand_Get_DAQ_List_Info->FIXED_EVENT = 0;

            env->responseSize = 6;
            break;
        }

        case XCP_FREE_DAQ: {
            break;
        }

        case XCP_ALLOC_DAQ: {
            break;
        }

        case XCP_ALLOC_ODT: {
            break;
        }

        case XCP_ALLOC_ODT_ENTRY: {
            eXcpCommand_DAQ_Alloc_ODR_Entry *request = (eXcpCommand_DAQ_Alloc_ODR_Entry *) env->canFrame.data;
            env->DAQ[request->DAQ_LIST_NUMBER].ODT_NUMBER[request->DAQ_LIST_NUMBER].ODT_ENTRIES_COUNT = request->ODT_ENTRIES_COUNT;
            break;
        }


        default:
            break;
    }

    if (env->isTransmitResponse) {
        env->ioCanFrame->transmit(env->ioCanFrame->env, env->response, env->responseSize, timeout);
    }
}

_Noreturn void CanXcpProcessing_ListenerTask(tCanSerialPortFrameXCP *env) {

    while (1) {
        vCanSerialPortFrameXcpReceive(env, 1000);
    }
}

void CanSerialPortFrameTp_Start(tCanSerialPortFrameXCP *env) {
    ThreadBlock_Start(env->T_can_Listener_XCP, env, CanXcpProcessing_ListenerTask);
}