/**
 * @brief Implementation of template class CanController<T>.
 */
#ifndef STA_CORE_CAN_SUBSCRIBABLE_TPP
#define STA_CORE_CAN_SUBSCRIBABLE_TPP

#ifndef STA_CORE_CAN_SUBSCRIBABLE_HPP
#error "Direct use of internal header. Use <sta/can/subscribable.hpp> instead"
#endif // !STA_CORE_CAN_SUBSCRIBABLE_HPP

#ifndef STA_STDLIB_DISABLE
#  include <algorithm> // fill_n
#endif // !STA_STDLIB_DISABLE


namespace sta
{
    template <typename T>
    bool SubscribableCanController<T>::subscribe(const CanFilterConfig * subscriptions, uint8_t num)
    {
        // Check bounds
        if (num > T::MAX_FILTER_COUNT)
            return false;

        // Clear previous subscriptions
        unsubscribeAll();

        for (uint8_t i = 0; i < num; ++i)
        {
            // Save handler callback
            filterCallbacks_[i] = subscriptions[i].callback;

            // Configure and enable filter
            T::configureFilter(i, subscriptions[i].filter, true);
        }

        return true;
    }

    template <typename T>
    void SubscribableCanController<T>::subscribeAll(CanRxCallback callback, uint8_t fifo)
    {
        uint8_t filterIdx = 0;

        // Clear previous subscriptions
        unsubscribeAll();

        // Setup default filter
        CanFilter filter{};
        filter.type = CanFilterIdFormat::ANY;
        filter.fifo = fifo;

        // Store callback
        filterCallbacks_[filterIdx] = callback;

        // Configure and enable default filter
        T::configureFilter(filterIdx, filter, true);
    }

    template <typename T>
    void SubscribableCanController<T>::unsubscribeAll()
    {
        // Disable all filters
        T::clearFilters();

        // Clear filter callbacks
#ifndef STA_STDLIB_DISABLE
        std::fill_n(filterCallbacks_, T::MAX_FILTER_COUNT, nullptr);
#else // STA_STDLIB_DISABLE
        for (uint8_t i = 0; i < T::MAX_FILTER_COUNT; ++i)
        {
            filterCallbacks_[i] = nullptr;
        }
#endif // STA_STDLIB_DISABLE
    }

    template <typename T>
    void SubscribableCanController<T>::receiveAndNotify(uint8_t fifo)
    {
        CanRxHeader header;
        uint8_t payload[T::MAX_PAYLOAD_SIZE];

        if (T::receiveFrame(fifo, &header, payload))
        {
            //displayFrameUART(frame);

            // Forward frame to filter callback
            if (fifo <= T::MAX_FILTER_COUNT && filterCallbacks_[header.filter])
            {
                filterCallbacks_[header.filter](header, payload);
            }
        }
    }

    template <typename T>
    void SubscribableCanController<T>::processMessages()
    {
        // Read RX interrupt flags
        uint32_t RFIF = T::getRxFifoFlags();

        if (RFIF != 0)
        {
            // Check all flags
            for (uint8_t fifo = 0; fifo < T::MAX_FIFO_COUNT; ++fifo)
            {
                // Process messages if flag is set
                if (RFIF & 0x1)
                {
                    receiveAndNotify(fifo);
                }

                // Shift next RX flag to LSB
                RFIF >>= 1;
            }
        }
    }
} // namespace sta


#endif // STA_CORE_CAN_SUBSCRIBABLE_TPP