#ifndef INCLUDE_TACOS_CAN_BUS_HPP_
#define INCLUDE_TACOS_CAN_BUS_HPP_

#include <sta/config.hpp>

#ifdef STA_CAN_BUS_ENABLE

#include <sta/tacos/thread.hpp>
#include <sta/rtos/queue.hpp>
#include <sta/rtos/system/can_bus.hpp>
#include <sta/debug/debug.hpp>

#include <sta/tacos/statemachine.hpp>


/**
 * @defgroup tacos_can_bus Can Bus Task
 * @ingroup tacos
 * @brief Can Bus task for TACOS.
 */

namespace sta
{
	namespace tacos
	{
        /**
		 * @brief Can Bus implementation for Tacos.
		 *
		 * @ingroup tacos_can_bus
		 */
		class CanBus : public TacosThread
        {
        public:

			CanBus();

            /**
			 * @brief Getter function for the singleton instance.
			 */
			static CanBus* instance()
			{
				static CGuard g;

				if (!_instance)
				{
					// Create the can bus singleton instance.
					CanBus::_instance = new CanBus();
				}

				return _instance;
			}

			/**
			 * @brief Place data message in CAN driver TX queue.
			 *
			 * @param msg       Message to transmit
			 * @param timeout   Timeout for placing message (0 = no wait, osWaitForever = blocking)
			 * @return          True if message was queued successfully
			 */
			bool queueCanBusMsg(const CanDataMsg & msg, uint32_t timeout);
			/**
			 * @brief Place system message in CAN driver TX queue.
			 *
			 * @param msg       Message to transmit
			 * @param timeout   Timeout for placing message (0 = no wait, osWaitForever = blocking)
			 * @return          True if message was queued successfully
			 */
			bool queueCanBusMsg(const CanSysMsg & msg, uint32_t timeout);

			/**
			 * @brief Retrieve data message from CAN driver TX queue.
			 *
			 * @param[out] msg	    Output address for retrieved message
			 * @param      timeout  Timeout for retrieving message (0 = no wait, osWaitForever = blocking)
			 * @return              True if message was retrieved successfully
			 */
			bool getCanBusMsg(CanDataMsg * msg, uint32_t timeout);

			/**
			 * @brief Retrieve system message from CAN driver TX queue.
			 *
			 * @param[out] msg      Destination for retrieved message
			 * @param      timeout  Timeout for retrieving message (0 = no wait, osWaitForever = blocking)
			 * @return              True if message was retrieved successfully
			 */
			bool getCanBusMsg(CanSysMsg * msg, uint32_t timeout);

			/**
			 *	@brief Buffers incoming message and sets event to notify the system.
			 *
			 *	@param fifo		FIFO number of the received message.
			 */
			void canCallback(uint32_t fifo);

			void init() override;
			void func() override;

        private:
            static CanBus * _instance;

			class CGuard
			{
			public:
			   ~CGuard()
			   {
				  if( NULL != CanBus::_instance )
				  {
					 delete CanBus::_instance;
					 CanBus::_instance = NULL;
				  }
			   }
			};

			sta::STM32CanController * canBusController_;

            //CanDataMsg canBusDataQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
            CanSysMsg* canBusSysQueueBuffer_[STA_RTOS_CAN_BUS_QUEUE_LENGTH];
            uint8_t bufferIndex;

            RtosQueue<CanSysMsg> canBusSysQueue_;
            RtosQueue<CanDataMsg> canBusDataQueue_;

            uint32_t canBusStack[256];

            AlpakaCanBus canBus_;

            static RtosEvent messageEvent;
        };


	void handleSysMessage(const sta::CanRxHeader & header, const uint8_t * payload);

	void handleDataMessage(const sta::IsotpMessage & msg);

    } /* namespace tacos */

} /* namespace sta */

#endif /* STA_CAN_BUS_ENABLE */
#endif /* INCLUDE_TACOS_CAN_BUS_HPP_ */