/**
 * @brief CMSIS RTOS2 queue implementation.
 */
#ifndef STA_RTOS2_QUEUE_HPP
#define STA_RTOS2_QUEUE_HPP

#include <sta/assert.hpp>

#include <cmsis_os2.h>


namespace sta
{
	template <typename T>
	class Rtos2Queue
	{
	public:
		using Message = T;
	public:
		Rtos2Queue(osMessageQueueId_t * handle);

		bool put(const Message & msg, uint32_t timeout = osWaitForever);
		bool get(Message * outMsg, uint32_t timeout = osWaitForever);

		uint32 available() const;

	private:
		osMessageQueueId_t * handle_;
	};
} // namespace sta


namespace sta
{
	template <typename T>
	Rtos2Queue<T>::Rtos2Queue(osMessageQueueId_t * handle)
		: handle_{handle}
	{
		STA_ASSERT(handle != nullptr);
	}

	template <typename T>
	bool Rtos2Queue<T>::put(const Message & msg, uint32_t timeout /* = osWaitForever */)
	{
		return (osOK == osMessageQueuePut(*handle_, &msg, 0, timeout));
	}

	template <typename T>
	bool Rtos2Queue<T>::get(Message * outMsg, uint32_t timeout /* = osWaitForever */)
	{
		uint8_t prio;
		return (osOK == osMessageQueueGet(*handle_, outMsg, &prio, timeout));
	}

	template <typename T>
	uint32 Rtos2Queue<T>::available() const
	{
		return osMessageQueueGetCount(*handle_);
	}
} // namespace sta


#endif // STA_RTOS2_QUEUE_HPP