//
// Created by cfif on 16.03.2024.
//
#include <SystemDelayInterface.h>
#include "CanSerialPort.h"
#include "memory.h"

static void vCanSerialPortInitNVICEnable(CAN_Module *CANx, uint8_t irqSubPriority) {

    NVIC_InitType NVIC_InitStructure;
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);

    if (CANx == CAN1) {
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
        NVIC_InitStructure.NVIC_IRQChannel = USB_LP_CAN1_RX0_IRQn;
    }

    if (CANx == CAN2) {
        NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
        NVIC_InitStructure.NVIC_IRQChannel = CAN2_RX0_IRQn;
    }

    NVIC_InitStructure.NVIC_IRQChannelSubPriority = irqSubPriority;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    if (CANx == CAN1) {
        NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, irqSubPriority);
    }

    if (CANx == CAN2) {
        NVIC_SetPriority(CAN2_RX0_IRQn, irqSubPriority);
    }
}

static void vCanSerialPortInitStructure(
        tCanSerialPortNation *env,
        CAN_Module *CANx,
        uint32_t rxBufferLength,
        uint32_t rxSnifferLength
) {
    env->rxDataQueue = osMessageQueueNew(rxBufferLength, 1, NULL);
    if (rxSnifferLength) {
        env->rxDataSnifferQueue = osMessageQueueNew(rxSnifferLength, 1, NULL);
    } else {
        env->rxDataSnifferQueue = 0;
    }
    env->can = CANx;
}

void CanSerialPortSetFilter(tCanSerialPortNation *env,
                            bool canFilterEnable,
                            uint32_t canFilterId,
                            uint32_t canFilterMask) {

    env->canFilterEnable = canFilterEnable;
    env->canFilterId = canFilterId;
    env->canFilterMask = canFilterMask;

    CAN_FilterInitType CAN_FilterInitStructure;

    CAN_FilterInitStructure.Filter_Num = 0;
    CAN_FilterInitStructure.Filter_Mode = CAN_Filter_IdMaskMode;
    CAN_FilterInitStructure.Filter_Scale = CAN_Filter_32bitScale;

    if (env->canTypeFrame == CAN_STD_ID) {
        CAN_FilterInitStructure.Filter_HighId = CAN_FILTER_STDID(env->canFilterId);
        CAN_FilterInitStructure.Filter_LowId = CAN_FILTER_STDID(env->canFilterId);
        CAN_FilterInitStructure.FilterMask_HighId = CAN_FILTER_STDMASK(env->canFilterMask);
        CAN_FilterInitStructure.FilterMask_LowId = CAN_FILTER_STDMASK(env->canFilterMask);
    }

    if (env->canTypeFrame == CAN_EXT_ID) {
        CAN_FilterInitStructure.Filter_HighId = CAN_FILTER_EXTID_H(env->canFilterId);
        CAN_FilterInitStructure.Filter_LowId = CAN_FILTER_EXTID_L(env->canFilterId);
        CAN_FilterInitStructure.FilterMask_HighId = CAN_FILTER_EXTMASK_H(env->canFilterMask);
        CAN_FilterInitStructure.FilterMask_LowId = CAN_FILTER_EXTMASK_L(env->canFilterMask);
    }

    CAN_FilterInitStructure.Filter_FIFOAssignment = CAN_FIFO0;

    if (env->canFilterEnable) {
        CAN_FilterInitStructure.Filter_Act = ENABLE;
    } else {
        CAN_FilterInitStructure.Filter_Act = DISABLE;
    }

    if (env->can == CAN1)
        CAN1_InitFilter(&CAN_FilterInitStructure);

    if (env->can == CAN2)
        CAN2_InitFilter(&CAN_FilterInitStructure);
}

void CanSerialPortSetId(tCanSerialPortNation *env, uint32_t id) {
    env->id = id;

    CanSerialPortSetFilter(env, env->canFilterEnable, env->canFilterId, env->canFilterMask);
}

void vCanSerialPortInit(
        tCanSerialPortNation *env,
        CAN_Module *CANx,
        eCanBaudRate canBaudRate,
        uint8_t irqSubPriority,
        eCanTypeFrame canTypeFrame,
        uint32_t canId,
        bool canFilterEnable,
        uint32_t canFilterId,
        uint32_t canFilterMask,
        uint32_t rxBufferLength,
        uint32_t rxSnifferLength
) {
    vCanSerialPortInitStructure(env, CANx, rxBufferLength, rxSnifferLength);

    vCanSerialPortInitNVICEnable(CANx, irqSubPriority);

    CAN_InitType CAN_InitStructure;
    CAN_DeInit(CANx);

    CAN_InitStruct(&CAN_InitStructure);
    CAN_InitStructure.TTCM = DISABLE;
    CAN_InitStructure.ABOM = DISABLE;
    CAN_InitStructure.AWKUM = DISABLE;
    CAN_InitStructure.NART = DISABLE;
    CAN_InitStructure.RFLM = DISABLE;
    CAN_InitStructure.TXFP = ENABLE;
    CAN_InitStructure.OperatingMode = CAN_Normal_Mode;

    if (canBaudRate == CAN_BAUD_RATE_1000K) {
        CAN_InitStructure.RSJW = CAN_RSJW_1tq;
        CAN_InitStructure.TBS1 = CAN_TBS1_5tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_3tq;
        CAN_InitStructure.BaudRatePrescaler = 4;
    }

    if (canBaudRate == CAN_BAUD_RATE_500K) {
        CAN_InitStructure.RSJW = CAN_RSJW_1tq;
        CAN_InitStructure.TBS1 = CAN_TBS1_5tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_2tq;
        CAN_InitStructure.BaudRatePrescaler = 9;
    }

    if (canBaudRate == CAN_BAUD_RATE_250K) {
/*
        CAN_InitStructure.RSJW = CAN_RSJW_1tq;
        CAN_InitStructure.TBS1 = CAN_TBS1_14tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_3tq;
        CAN_InitStructure.BaudRatePrescaler = 8;
*/
        CAN_InitStructure.RSJW = CAN_RSJW_1tq;
        CAN_InitStructure.TBS1 = CAN_TBS1_4tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_3tq;
        CAN_InitStructure.BaudRatePrescaler = 18;
    }

    if (canBaudRate == CAN_BAUD_RATE_125K) {
        /*
        CAN_InitStructure.RSJW = CAN_RSJW_1tq;
        CAN_InitStructure.TBS1 = CAN_TBS1_6tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_5tq;
        CAN_InitStructure.BaudRatePrescaler = 24;
*/
       CAN_InitStructure.RSJW = CAN_RSJW_1tq;
       CAN_InitStructure.TBS1 = CAN_TBS1_13tq;
       CAN_InitStructure.TBS2 = CAN_TBS2_4tq;
       CAN_InitStructure.BaudRatePrescaler = 16;
    }

    CAN_Init(CANx, &CAN_InitStructure);

    env->canTypeFrame = canTypeFrame;

    CanSerialPortSetId(env, canId);
    CanSerialPortSetFilter(env, canFilterEnable, canFilterId, canFilterMask);

    CAN_INTConfig(CANx, CAN_INT_FMP0, ENABLE);

}


void CanSerialPortIrqRxProcessing(tCanSerialPortNation *env) {
    CanRxMessage CAN_RxMessage;
    CAN_ReceiveMessage(env->can, CAN_FIFO0, &CAN_RxMessage);

    for (uint8_t i = 0; i < CAN_RxMessage.DLC; ++i) {
        osMessageQueuePut(env->rxDataQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
        if (env->rxDataSnifferQueue) {
            osMessageQueuePut(env->rxDataSnifferQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
        }
    }

//    if (env->can->RFF0 & CAN_RFF0_FFULL0) {
//        CAN_ReceiveMessage(env->can, CAN_FIFO0, &CAN_RxMessage);
//
//        for (uint8_t i = 0; i < CAN_RxMessage.DLC; ++i) {
//            osMessageQueuePut(env->rxDataQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
//            if (env->rxDataSnifferQueue) {
//                osMessageQueuePut(env->rxDataSnifferQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
//            }
//        }
//    }
//
//    if (env->can->RFF1 & CAN_RFF1_FFULL1) {
//
//        CAN_ReceiveMessage(env->can, CAN_FIFO1, &CAN_RxMessage);
//
//        for (uint8_t i = 0; i < CAN_RxMessage.DLC; ++i) {
//            osMessageQueuePut(env->rxDataQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
//            if (env->rxDataSnifferQueue) {
//                osMessageQueuePut(env->rxDataSnifferQueue, &CAN_RxMessage.Data[i], 0x0, 0U);
//            }
//        }
//    }

}

static uint16_t vSerialPortReceiveQueue(
        tCanSerialPortNation *env, uint8_t *data, uint16_t size, uint32_t timeout, osMessageQueueId_t queueId
) {
    uint16_t received = 0;

    if (timeout) {
        uint32_t endMs = SystemGetMs() + timeout;
        uint32_t leftMs;

        while (size && ((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
            leftMs = endMs - SystemGetMs();
            if (osMessageQueueGet(queueId, data, NULL, leftMs) == osOK) {
                --size;
                ++received;
                ++data;
            }
        }
    } else {
        while (size) {
            if (osMessageQueueGet(queueId, data, NULL, 0) == osOK) {
                --size;
                ++received;
                ++data;
            } else {
                return received;
            }
        }
    }

    return received;
}

static uint16_t vCanSerialPortReceive(tCanSerialPortNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortReceiveQueue(env, data, size, timeout, env->rxDataQueue);
}

static uint16_t vCanSerialPortReceiveSniffer(tCanSerialPortNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {

    return env->rxDataSnifferQueue
           ? vSerialPortReceiveQueue(env, data, size, timeout, env->rxDataSnifferQueue)
           : 0;
}

uint16_t vCanSerialPortTransmit(tCanSerialPortNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    uint16_t sent = 0;
    uint32_t endMs = SystemGetMs() + timeout;

    uint16_t fullSize = size / 8;
    uint8_t tailSize = size % 8;

    CanTxMessage TxMessage;

    TxMessage.StdId = env->id;

    if (env->canTypeFrame == CAN_STD_ID)
        TxMessage.IDE = CAN_ID_STD;

    if (env->canTypeFrame == CAN_EXT_ID)
        TxMessage.IDE = CAN_ID_EXT;

    TxMessage.RTR = CAN_RTRQ_DATA;
    TxMessage.DLC = 8;

    while (size && ((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {

        uint16_t len = 0;
        for (uint16_t i = 0; i < fullSize; ++i) {

            memcpy(TxMessage.Data, &data[len], 8);
            len += 8;

            CAN_TransmitMessage(env->can, &TxMessage);

            while (CAN_TransmitSTS(env->can, 0) != CANTXSTSOK) {
                if (!((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
                    return sent;
                }
            }

            sent += 8;
            size -= 8;
        }

        if (tailSize) {
            TxMessage.DLC = tailSize;
            memcpy(TxMessage.Data, &data[len], tailSize);

            CAN_TransmitMessage(env->can, &TxMessage);

            while (CAN_TransmitSTS(env->can, 0) != CANTXSTSOK) {
                if (!((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
                    return sent;
                }
            }

            sent += tailSize;
            size -= tailSize;
        }

    }

    return sent;
}

tSerialPortIO CanPort_GetIo(tCanSerialPortNation *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vCanSerialPortReceive,
            .transmit = (SerialPortIOTransaction) vCanSerialPortTransmit
    };
    return io;
}

tSerialPortIO CanPort_GetSnifferIo(tCanSerialPortNation *env) {
    tSerialPortIO io = {
            .env = env,
            .receive = (SerialPortIOTransaction) vCanSerialPortReceiveSniffer,
            .transmit = (SerialPortIOTransaction) vCanSerialPortTransmit
    };
    return io;
}