#ifndef INCLUDE_TACOS_CAN_BUS_HPP_
#define INCLUDE_TACOS_CAN_BUS_HPP_

#include <sta/config.hpp>

#ifdef STA_RTOS_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(CAN_HandleTypeDef * controller);

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

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

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

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

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

            uint32_t canBusStack[256];

            AlpakaCanBus canBus_;


        };


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

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

    } /* namespace tacos */

} /* namespace sta */

#endif /* STA_RTOS_CAN_BUS_ENABLE */
#endif /* INCLUDE_TACOS_CAN_BUS_HPP_ */