#include <sta/devices/stm32/can.hpp>
#ifdef STA_STM32_CAN_ENABLED

#include <sta/debug/assert.hpp>
#include <sta/lang.hpp>


namespace sta
{
    STM32CanController::STM32CanController(CAN_HandleTypeDef * handle)
        : handle_{handle}
    {
        initFilters();
    }


    void STM32CanController::enableRxInterrupts()
    {
        HAL_CAN_ActivateNotification(handle_,
            CAN_IT_RX_FIFO0_MSG_PENDING | CAN_IT_RX_FIFO1_MSG_PENDING
        );
    }


    void STM32CanController::start()
    {
        HAL_CAN_Start(handle_);
    }

    void STM32CanController::stop()
    {
        HAL_CAN_Stop(handle_);
    }


    bool STM32CanController::sendFrame(const CanTxHeader & header, const uint8_t * payload)
    {
        STA_ASSERT_MSG(header.payloadLength <= 8, "CAN 2.0B payload size exceeded");

        CAN_TxHeaderTypeDef halHeader;

        if (header.id.format == CanIdFormat::STD)
        {
            halHeader.StdId = header.id.sid & 0x7FF;
            halHeader.IDE   = CAN_ID_STD;
        }
        else
        {
            // Combine SID and EID
            halHeader.ExtId = ((header.id.sid & 0x7FF) << 18) | (header.id.eid & 0x3FFFF);
            halHeader.IDE   = CAN_ID_EXT;
        }

        halHeader.DLC = header.payloadLength;

        uint32_t mailbox; // Don't care
        return (HAL_OK == HAL_CAN_AddTxMessage(handle_, &halHeader, const_cast<uint8_t *>(payload), &mailbox));
    }

    bool STM32CanController::receiveFrame(uint8_t fifo, CanRxHeader * header, uint8_t * payload)
    {
        // Check if message is available
        if (HAL_CAN_GetRxFifoFillLevel(handle_, fifo) == 0)
            return false;

        // Retrieve message
        CAN_RxHeaderTypeDef halHeader;
        HAL_CAN_GetRxMessage(handle_, fifo, &halHeader, payload);

        if (halHeader.IDE == CAN_ID_STD)
        {
            header->id.format   = CanIdFormat::STD;
            header->id.sid      = halHeader.StdId;
            header->id.eid      = 0;
        }
        else
        {
            header->id.format   = CanIdFormat::EXT;
            // Separate SID and EID
            header->id.sid      = (halHeader.ExtId >> 18);
            header->id.eid      = halHeader.ExtId & 0x3FFFF;
        }
        // No conversion required for CAN 2B standard
        header->payloadLength  = halHeader.DLC;
        header->timestamp      = halHeader.Timestamp;
        header->filter         = halHeader.FilterMatchIndex;

        return true;
    }

    uint32_t STM32CanController::getRxFifoFlags()
    {
        //
        return (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) != 0)
                | (HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1) != 0) << 1;
    }


    void STM32CanController::configureFilter(uint8_t idx, const CanFilter & filter, bool active /* = false */)
    {
        CAN_FilterTypeDef * config = &filters_[idx];

        if (filter.type == CanFilterIdFormat::STD)
        {
            config->FilterIdHigh        = 0;
            config->FilterIdLow         = filter.obj.sid & 0x7FF;
            config->FilterMaskIdHigh    = 0;
            config->FilterMaskIdLow     = filter.mask.sid & 0x7FF;
        }
        else
        {
            config->FilterIdHigh        = ((filter.obj.sid & 0x7FF) << 2) | ((filter.obj.eid >> 16) & 0x3);
            config->FilterIdLow         = filter.obj.eid & 0xFFFF;
            config->FilterMaskIdHigh    = ((filter.mask.sid & 0x7FF) << 2) | ((filter.mask.eid >> 16) & 0x3);
            config->FilterMaskIdLow     = filter.mask.eid & 0xFFFF;
        }

        config->FilterFIFOAssignment    = filter.fifo;
        config->FilterActivation        = (active ? CAN_FILTER_ENABLE : CAN_FILTER_DISABLE);

        HAL_CAN_ConfigFilter(handle_, config);
    }

    void STM32CanController::enableFilter(uint8_t idx)
    {
        CAN_FilterTypeDef * config = &filters_[idx];

        config->FilterActivation = CAN_FILTER_ENABLE;

        HAL_CAN_ConfigFilter(handle_, config);
    }

    void STM32CanController::disableFilter(uint8_t idx)
    {
        CAN_FilterTypeDef * config = &filters_[idx];

        config->FilterActivation = CAN_FILTER_DISABLE;

        HAL_CAN_ConfigFilter(handle_, config);
    }

    void STM32CanController::clearFilters()
    {
        for (uint32_t i = 0; i < MAX_FILTER_COUNT; ++i)
        {
            CAN_FilterTypeDef * config = &filters_[i];

            // Only disable active filters
            if (config->FilterActivation == CAN_FILTER_ENABLE)
            {
                config->FilterActivation = CAN_FILTER_DISABLE;
                HAL_CAN_ConfigFilter(handle_, config);
            }
        }
    }


    void STM32CanController::initFilters()
    {
        for (uint32_t i = 0; i < MAX_FILTER_COUNT; ++i)
        {
            CAN_FilterTypeDef * config = &filters_[i];

            config->FilterBank           = i;
            config->FilterMode           = CAN_FILTERMODE_IDMASK;
            config->FilterScale          = CAN_FILTERSCALE_32BIT;
            config->FilterActivation     = CAN_FILTER_DISABLE;
            config->SlaveStartFilterBank = MAX_FILTER_COUNT;
        }
    }

    CanPendingRxFifos STM32CanController::getPendingRxFifos(){
    	CanPendingRxFifos pendingFifos(42, 3);

        // Example implementation:
        //pendingFifos.fifo0Pending = HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO0) != 0;
        //pendingFifos.fifo1Pending = HAL_CAN_GetRxFifoFillLevel(handle_, CAN_RX_FIFO1) != 0;

        return pendingFifos;
    }

    uint8_t STM32CanController::maxFilterCount() const{
        return MAX_FILTER_COUNT;
    }

    uint8_t STM32CanController::maxFifoCount() const {
        return MAX_FIFO_COUNT;
    }

    uint8_t STM32CanController::maxPayloadSize() const {
        return MAX_PAYLOAD_SIZE;
    }

} // namespace sta


#ifdef STA_STM32_CAN_GLOBAL

#include <can.h>

namespace sta
{
    STM32CanController CanBus(&STA_STM32_CAN_GLOBAL);

    STA_WEAK
    void CanBus_RxPendingCallback()
    {}
} // namespace sta


extern "C"
{
    void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
    {
        if (hcan == &STA_STM32_CAN_GLOBAL)
        {
            sta::CanBus_RxPendingCallback();
        }
    }

    void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
    {
        if (hcan == &STA_STM32_CAN_GLOBAL)
        {
            sta::CanBus_RxPendingCallback();
        }
    }
}

#endif // STA_STM32_CAN_GLOBAL


#endif // STA_STM32_CAN_ENABLED