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

#define  CAN_FRAME_FILTER_STDID(STDID)        ((STDID & 0x7FF) << 5)
#define  CAN_FRAME_FILTER_EXTID_H(EXTID)      ((uint16_t)(((EXTID << 3) | CAN_ID_EXT) >> 16))
#define  CAN_FRAME_FILTER_EXTID_L(EXTID)      ((uint16_t)(( EXTID << 3) | CAN_ID_EXT))


#define  CAN_FRAME_FILTER_STDMASK(STDMASK)    ((STDMASK & 0x7FF) << 5)
#define  CAN_FRAME_FILTER_EXTMASK_H(EXTMASK)  ((uint16_t)(((EXTMASK << 3) >> 16)))
#define  CAN_FRAME_FILTER_EXTMASK_L(EXTMASK)  ((uint16_t)(EXTMASK << 3))


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

    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);

    if (CANx == CAN1) {
        NVIC_EnableIRQ(CAN1_SCE_IRQn);
        NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
        NVIC_EnableIRQ(CAN1_RX1_IRQn);
        NVIC_SetPriority(CAN1_SCE_IRQn, irqSubPriority);
        NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, irqSubPriority);
        NVIC_SetPriority(CAN1_RX1_IRQn, irqSubPriority);

        CAN_INTConfig(CAN1, CAN_INT_FMP0, ENABLE);
        CAN_INTConfig(CAN1, CAN_INT_FMP1, ENABLE);

        CAN_INTConfig(CAN1, CAN_INT_LEC, ENABLE);
//        CAN_INTConfig(CAN1, CAN_INT_ERR, ENABLE);
    }

    if (CANx == CAN2) {
        NVIC_EnableIRQ(CAN2_SCE_IRQn);
        NVIC_EnableIRQ(CAN2_RX0_IRQn);
        NVIC_EnableIRQ(CAN2_RX1_IRQn);
        NVIC_SetPriority(CAN2_SCE_IRQn, irqSubPriority);
        NVIC_SetPriority(CAN2_RX0_IRQn, irqSubPriority);
        NVIC_SetPriority(CAN2_RX1_IRQn, irqSubPriority);

        CAN_INTConfig(CAN2, CAN_INT_FMP0, ENABLE);
        CAN_INTConfig(CAN2, CAN_INT_FMP1, ENABLE);

        CAN_INTConfig(CAN2, CAN_INT_LEC, ENABLE);
//        CAN_INTConfig(CAN2, CAN_INT_ERR, ENABLE);
    }


}

void CanSerialPortFrameSetFilter(tCanSerialPortFrameNation *env,
                                 bool canFilterEnable,
                                 uint32_t canFilter0Id,
                                 uint32_t canFilter0Mask,
                                 uint32_t canFilter1Id,
                                 uint32_t canFilter1Mask) {

    CAN_FilterInitType CAN_FilterInitStructure0;

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

    if (env->canTypeFrame == CAN_STD_ID) {
        CAN_FilterInitStructure0.Filter_HighId = CAN_FRAME_FILTER_STDID(canFilter0Id);
        CAN_FilterInitStructure0.Filter_LowId = CAN_FRAME_FILTER_STDID(canFilter0Id);
        CAN_FilterInitStructure0.FilterMask_HighId = CAN_FRAME_FILTER_STDMASK(canFilter0Mask);
        CAN_FilterInitStructure0.FilterMask_LowId = CAN_FRAME_FILTER_STDMASK(canFilter0Mask);
    }

    if (env->canTypeFrame == CAN_EXT_ID) {
        CAN_FilterInitStructure0.Filter_HighId = CAN_FRAME_FILTER_EXTID_H(canFilter0Id);
        CAN_FilterInitStructure0.Filter_LowId = CAN_FRAME_FILTER_EXTID_L(canFilter0Id);
        CAN_FilterInitStructure0.FilterMask_HighId = CAN_FRAME_FILTER_EXTMASK_H(canFilter0Mask);
        CAN_FilterInitStructure0.FilterMask_LowId = CAN_FRAME_FILTER_EXTMASK_L(canFilter0Mask);
    }

    CAN_FilterInitStructure0.Filter_FIFOAssignment = CAN_FIFO0;

    if (canFilterEnable) {
        CAN_FilterInitStructure0.Filter_Act = ENABLE;
    } else {
        CAN_FilterInitStructure0.Filter_Act = DISABLE;
    }


    CAN_FilterInitType CAN_FilterInitStructure1;

    CAN_FilterInitStructure1.Filter_Num = 1;
    CAN_FilterInitStructure1.Filter_Mode = CAN_Filter_IdMaskMode;
    CAN_FilterInitStructure1.Filter_Scale = CAN_Filter_32bitScale;

    if (env->canTypeFrame == CAN_STD_ID) {
        CAN_FilterInitStructure1.Filter_HighId = CAN_FRAME_FILTER_STDID(canFilter1Id);
        CAN_FilterInitStructure1.Filter_LowId = CAN_FRAME_FILTER_STDID(canFilter1Id);
        CAN_FilterInitStructure1.FilterMask_HighId = CAN_FRAME_FILTER_STDMASK(canFilter1Mask);
        CAN_FilterInitStructure1.FilterMask_LowId = CAN_FRAME_FILTER_STDMASK(canFilter1Mask);
    }

    if (env->canTypeFrame == CAN_EXT_ID) {
        CAN_FilterInitStructure1.Filter_HighId = CAN_FRAME_FILTER_EXTID_H(canFilter1Id);
        CAN_FilterInitStructure1.Filter_LowId = CAN_FRAME_FILTER_EXTID_L(canFilter1Id);
        CAN_FilterInitStructure1.FilterMask_HighId = CAN_FRAME_FILTER_EXTMASK_H(canFilter1Mask);
        CAN_FilterInitStructure1.FilterMask_LowId = CAN_FRAME_FILTER_EXTMASK_L(canFilter1Mask);
    }

    CAN_FilterInitStructure1.Filter_FIFOAssignment = CAN_FIFO1;

    if (canFilterEnable) {
        CAN_FilterInitStructure1.Filter_Act = ENABLE;
    } else {
        CAN_FilterInitStructure1.Filter_Act = DISABLE;
    }


    if (env->can == CAN1) {
        CAN1_InitFilter(&CAN_FilterInitStructure0);
        CAN1_InitFilter(&CAN_FilterInitStructure1);
    }

    if (env->can == CAN2) {
        CAN2_InitFilter(&CAN_FilterInitStructure0);
        CAN2_InitFilter(&CAN_FilterInitStructure1);
    }
}

void CanSerialPortFrameSetId(tCanSerialPortFrameNation *env, uint32_t id) {
    env->id = id;
}

void CanSerialPortFrameSetId1(tCanSerialPortFrameNation *env, uint32_t id) {
    env->id1 = id;
}

void CanSerialPortFrameSetType(tCanSerialPortFrameNation *env, eCanTypeFrame canTypeFrame) {
    env->canTypeFrame = canTypeFrame;
}

static void vCanSerialPortFrameInitStructure(
        tCanSerialPortFrameNation *env,
        CAN_Module *CANx,
        uint32_t rxBufferLength0,
        uint32_t rxSnifferLength0,
        uint32_t rxBufferLength1,
        uint32_t rxSnifferLength1
) {
    env->rxDataQueue0 = osMessageQueueNew(rxBufferLength0, sizeof(CanRxMessage), NULL);
    if (rxSnifferLength0) {
        env->rxDataSnifferQueue0 = osMessageQueueNew(rxSnifferLength0, sizeof(CanRxMessage), NULL);
    } else {
        env->rxDataSnifferQueue0 = 0;
    }

    env->rxDataQueue1 = osMessageQueueNew(rxBufferLength1, sizeof(CanRxMessage), NULL);
    if (rxSnifferLength1) {
        env->rxDataSnifferQueue1 = osMessageQueueNew(rxSnifferLength1, sizeof(CanRxMessage), NULL);
    } else {
        env->rxDataSnifferQueue1 = 0;
    }
}

void vCanSerialPortFrameInit(
        tCanSerialPortFrameNation *env,
        CAN_Module *CANx,
        eCanBaudRate canBaudRate,
        uint8_t irqSubPriority,
        eCanTypeFrame canTypeFrame,
        uint32_t canId,
        uint32_t rxBufferLength0,
        uint32_t rxSnifferLength0,
        uint32_t rxBufferLength1,
        uint32_t rxSnifferLength1
) {

    env->can = CANx;

    if (!env->reInit) {
        env->reInit = true;
        vCanSerialPortFrameInitStructure(env, CANx, rxBufferLength0, rxSnifferLength0, rxBufferLength1,
                                         rxSnifferLength1);

        CanSerialPortFrameSetType(env, canTypeFrame);
        CanSerialPortFrameSetId(env, canId);

        env->canBaudRate = canBaudRate;
    }


    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_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_13tq;
        CAN_InitStructure.TBS2 = CAN_TBS2_4tq;
        CAN_InitStructure.BaudRatePrescaler = 16;
    }

    CAN_Init(CANx, &CAN_InitStructure);

    CanSerialPortFrameSetFilter(env, true, 0, 0, 0, 0);

    vCanSerialPortFrameInitNVICEnable(CANx, irqSubPriority);
}


void CanSerialPortFrameIrqRxProcessing0(tCanSerialPortFrameNation *env) {
    CanRxMessage rx_message_struct;

    CAN_ReceiveMessage(env->can, CAN_FIFO0, &rx_message_struct);

    osMessageQueuePut(env->rxDataQueue0, &rx_message_struct, 0x0, 0U);

    if (env->rxDataSnifferQueue0) {
        osMessageQueuePut(env->rxDataSnifferQueue0, &rx_message_struct, 0x0, 0U);
    }

}

void CanSerialPortFrameIrqRxProcessing1(tCanSerialPortFrameNation *env) {
    CanRxMessage rx_message_struct;

    CAN_ReceiveMessage(env->can, CAN_FIFO1, &rx_message_struct);

    osMessageQueuePut(env->rxDataQueue1, &rx_message_struct, 0x0, 0U);

    if (env->rxDataSnifferQueue1) {
        osMessageQueuePut(env->rxDataSnifferQueue1, &rx_message_struct, 0x0, 0U);
    }

}

static uint16_t vSerialPortFrameReceiveQueue(
        tCanSerialPortFrameNation *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
vCanSerialPortFrameReceive0(tCanSerialPortFrameNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortFrameReceiveQueue(env, data, size, timeout, env->rxDataQueue0);
}


static uint16_t
vCanSerialPortFrameReceive1(tCanSerialPortFrameNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {
    return vSerialPortFrameReceiveQueue(env, data, size, timeout, env->rxDataQueue1);
}


static uint16_t
vCanSerialPortFrameReceiveSniffer0(tCanSerialPortFrameNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {

    return env->rxDataSnifferQueue0
           ? vSerialPortFrameReceiveQueue(env, data, size, timeout, env->rxDataSnifferQueue0)
           : 0;
}

static uint16_t
vCanSerialPortFrameReceiveSniffer1(tCanSerialPortFrameNation *env, uint8_t *data, uint16_t size, uint32_t timeout) {

    return env->rxDataSnifferQueue1
           ? vSerialPortFrameReceiveQueue(env, data, size, timeout, env->rxDataSnifferQueue1)
           : 0;
}

uint8_t can_message_transmitExt(CAN_Module* CANx, CanTxMessage* TxMessage, can_tx_mailbox_num_type mailbox)
{
    uint8_t transmit_mailbox = CAN_TxSTS_NoMailBox;

    if (mailbox == CAN_TX_MAILBOX0) {
        if (CANx->TSTS & CAN_TSTS_TMEM0) {
            transmit_mailbox = CAN_TX_MAILBOX0;
        } else {
            transmit_mailbox = CAN_TxSTS_NoMailBox;
        }
    }

    if (mailbox == CAN_TX_MAILBOX1) {
        if (CANx->TSTS & CAN_TSTS_TMEM1) {
            transmit_mailbox = CAN_TX_MAILBOX1;
        } else {
            transmit_mailbox = CAN_TxSTS_NoMailBox;
        }
    }

    if (mailbox == CAN_TX_MAILBOX2) {
        if (CANx->TSTS & CAN_TSTS_TMEM2) {
            transmit_mailbox = CAN_TX_MAILBOX2;
        } else {
            transmit_mailbox = CAN_TxSTS_NoMailBox;
        }
    }


    if (transmit_mailbox != CAN_TxSTS_NoMailBox)
    {
        CANx->sTxMailBox[transmit_mailbox].TMI &= 1;
        if (TxMessage->IDE == CAN_Standard_Id)
        {
            assert_param(IS_CAN_STDID(TxMessage->StdId));
            CANx->sTxMailBox[transmit_mailbox].TMI |= ((TxMessage->StdId << 21) | TxMessage->RTR);
        }
        else
        {
            assert_param(IS_CAN_EXTID(TxMessage->ExtId));
            CANx->sTxMailBox[transmit_mailbox].TMI |= ((TxMessage->ExtId << 3) | TxMessage->IDE | TxMessage->RTR);
        }

        TxMessage->DLC &= (uint8_t)0x0000000F;
        CANx->sTxMailBox[transmit_mailbox].TMDT &= (uint32_t)0xFFFFFFF0;
        CANx->sTxMailBox[transmit_mailbox].TMDT |= TxMessage->DLC;

        CANx->sTxMailBox[transmit_mailbox].TMDL =
                (((uint32_t)TxMessage->Data[3] << 24) | ((uint32_t)TxMessage->Data[2] << 16)
                 | ((uint32_t)TxMessage->Data[1] << 8) | ((uint32_t)TxMessage->Data[0]));

        CANx->sTxMailBox[transmit_mailbox].TMDH =
                (((uint32_t)TxMessage->Data[7] << 24) | ((uint32_t)TxMessage->Data[6] << 16)
                 | ((uint32_t)TxMessage->Data[5] << 8) | ((uint32_t)TxMessage->Data[4]));

        CANx->sTxMailBox[transmit_mailbox].TMI |= 1;
    }

    return transmit_mailbox;
}


uint16_t vCanSerialPortFrameTransmit(tCanSerialPortFrameNation *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;

    uint8_t transmit_mailbox;
    CanTxMessage TxMessage;


    if (env->canTypeFrame == CAN_STD_ID) {
        TxMessage.StdId = env->id;
        TxMessage.ExtId = 0;
        TxMessage.IDE = CAN_ID_STD;
    }

    if (env->canTypeFrame == CAN_EXT_ID) {
        TxMessage.StdId = 0;
        TxMessage.ExtId = env->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;

            transmit_mailbox = can_message_transmitExt(env->can, &TxMessage, CAN_TX_MAILBOX0);

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

            transmit_mailbox = can_message_transmitExt(env->can, &TxMessage, CAN_TX_MAILBOX0);

            uint8_t status = CAN_TransmitSTS(env->can, (can_tx_mailbox_num_type) transmit_mailbox);

            while (status != CANTXSTSOK) {

                status = CAN_TransmitSTS(env->can, (can_tx_mailbox_num_type) transmit_mailbox);

                if (!((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
                    return sent;
                }

            }

            sent += tailSize;
            size -= tailSize;
        }

    }

    return sent;
}

uint16_t vCanSerialPortFrameTransmit1(tCanSerialPortFrameNation *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;

    uint8_t transmit_mailbox;
    CanTxMessage TxMessage;


    if (env->canTypeFrame == CAN_STD_ID) {
        TxMessage.StdId = env->id;
        TxMessage.ExtId = 0;
        TxMessage.IDE = CAN_ID_STD;
    }

    if (env->canTypeFrame == CAN_EXT_ID) {
        TxMessage.StdId = 0;
        TxMessage.ExtId = env->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;

            transmit_mailbox = can_message_transmitExt(env->can, &TxMessage, CAN_TX_MAILBOX1);

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

            transmit_mailbox = can_message_transmitExt(env->can, &TxMessage, CAN_TX_MAILBOX1);

            uint8_t status = CAN_TransmitSTS(env->can, (can_tx_mailbox_num_type) transmit_mailbox);

            while (status != CANTXSTSOK) {

                status = CAN_TransmitSTS(env->can, (can_tx_mailbox_num_type) transmit_mailbox);

                if (!((timeout == SystemWaitForever) || (endMs > SystemGetMs()))) {
                    return sent;
                }

            }

            sent += tailSize;
            size -= tailSize;
        }

    }

    return sent;
}

tSerialPortFrameIO CanPortFrame_GetIo(tCanSerialPortFrameNation *env) {
    tSerialPortFrameIO io = {
            .env = env,
            .receive0 = (SerialPortFrameIOTransaction) vCanSerialPortFrameReceive0,
            .receive1 = (SerialPortFrameIOTransaction) vCanSerialPortFrameReceive1,
            .transmit = (SerialPortFrameIOTransaction) vCanSerialPortFrameTransmit,
            .transmit1 = (SerialPortFrameIOTransaction) vCanSerialPortFrameTransmit1
    };
    return io;
}

tSerialPortFrameIO CanPortFrame_GetSnifferIo(tCanSerialPortFrameNation *env) {
    tSerialPortFrameIO io = {
            .env = env,
            .receive0 = (SerialPortFrameIOTransaction) vCanSerialPortFrameReceiveSniffer0,
            .receive1 = (SerialPortFrameIOTransaction) vCanSerialPortFrameReceiveSniffer1,
            .transmit = (SerialPortFrameIOTransaction) vCanSerialPortFrameTransmit,
            .transmit1 = (SerialPortFrameIOTransaction) vCanSerialPortFrameTransmit1
    };
    return io;
}