#include <stdint.h>
#include <string.h>
#include "cmsis_os.h"
#include "mbed_error.h"

bool handlerMode()
{
return __get_IPSR() != 0;
}

// Очередь с ленивой инициализацией - инициализируется в момент первого использования.
// Сделано для обхода проблемы с неопределенным порядком инициализации глобальных объектов.
// Очереди создаются глобально и менеджер памяти создается глобально.
// Очередь использует выделение памяти в куче, поэтому должна быть создана после менеджера памяти.
// Поэтому и сделана ленивая инициализация.
// Бывают случаи, когда первое использование очереди происходит в прерывании, что недопустимо.
// На этот случай, нужно вызывать метод create() явно, до первого использования.
template<typename T, size_t N>
class Queue
{
public:
Queue(const char * name = NULL)
: mName(name)
{}

/** Put a message in a Queue.
@param data data.
@param millisec timeout value or 0 in case of no time-out. (default: 0)
@return status code that indicates the execution status of the function.
*/
bool put(const T & data, uint32_t millisec = 0)
{
if (!mQueueId)
{
create();
}
portBASE_TYPE taskWoken = pdFALSE; // не понятно, зачем это надо
if (handlerMode())
{
if (xQueueSendToBackFromISR(mQueueId, reinterpret_cast<const void *>(&data), &taskWoken) == pdTRUE)
{
portEND_SWITCHING_ISR(taskWoken);
return true;
}
}
else
{
TickType_t ticks = millisec / portTICK_PERIOD_MS;

if (ticks == 0)
{
ticks = 1;
}

if (xQueueSendToBack(mQueueId, reinterpret_cast<const void *>(&data), ticks) == pdTRUE)
{
return true;
}
}
return false;
}

/** Get a message or Wait for a message from a Queue.
@param millisec timeout value or 0 in case of no time-out. (default: osWaitForever).
@return event information that includes the message and the status code.
*/
bool hasMessage(uint32_t millisec = osWaitForever)
{
if (!mQueueId)
{
create();
}
bool hasMessage = false;
portBASE_TYPE taskWoken = pdFALSE;
if (handlerMode())
{
hasMessage = xQueueReceiveFromISR(mQueueId, reinterpret_cast<void *>(&mData), &taskWoken) == pdTRUE;
portEND_SWITCHING_ISR(taskWoken);
}
else
{
TickType_t ticks = 0;
if (millisec == osWaitForever)
{
ticks = portMAX_DELAY;
}
else if (millisec != 0)
{
ticks = millisec / portTICK_PERIOD_MS;
if (ticks == 0)
{
ticks = 1;
}
}

hasMessage = xQueueReceive(mQueueId, reinterpret_cast<void *>(&mData), ticks) == pdTRUE;
}
return hasMessage;
}

const T & message() const
{
return mData;
}

T & message()
{
return mData;
}

size_t spaceAvailable() const
{
return uxQueueSpacesAvailable(mQueueId);
}

// нельзя вызывать из прерывания.
void create()
{
// неплохо бы проверять, что мы находимся не в прерывании
mQueueId = xQueueCreate(N, sizeof(T));

if (!mQueueId)
{
error("Error initialising the queue object\n");
}
if (mName)
{
vQueueAddToRegistry(mQueueId, mName);
}
}

private:
const char * mName;
QueueHandle_t mQueueId;
T mData;
};

#include <algorithm>

template <size_t N>
class Variant
{
public:
Variant()
{}

template <typename T>
Variant(const T & t)
{
std::copy(reinterpret_cast<const uint8_t *>(&t), reinterpret_cast<const uint8_t *>(&t) + sizeof(mData), mData);
}

template <typename T>
T get() const
{
return *reinterpret_cast<const T *>(mData);
}

template <typename T>
T * value()
{
return reinterpret_cast<T *>(mData);
}

Variant & operator= (const Variant & other)
{
std::copy(other.mData, other.mData + sizeof(mData), mData);
return *this;
}

float toFloat() const
{
return get<float>();
}

uint32_t toUInt32() const
{
return get<uint32_t>();
}


private:
uint8_t mData[N];
};

struct Message
{
...
};

typedef Variant<sizeof(Message)> MessageVariant; // знаю, что структура Message самая большая из помещаемых в MessageVariant
Queue<MessageVariant, 10> queue;

...
if (queue.hasMessage())
{
const MessageVariant & msgVar = queue.message();
const Message & msg = msgVar .data().get<Message>();
}

public:
typedef uint32_t BufferT;

// Fifo item base class. Must be POD
class ItemBase{
public:
// pointer to function that knows how to call specific lambda
typedef void(*WrapperT)(ItemBase*, TArgs...);

void invoke(TArgs ... args){
wrapper_(this, args...);
}

// ------------------------------------------
static size_t mark_unused(size_t size){
return size | UnusedMask;
}

size_t size()const{ return size_ & ~UnusedMask; }
bool isUsed()const{ return (size_ & UnusedMask) == 0; }

enum{ UnusedMask = 1<<(sizeof(size_t)*8-1) };

// ------------------------------------------
protected:
size_t size_; // in sizeof(BufferT) units
WrapperT wrapper_;
};

private:
// Constructors are private.
// You cant create objects, create VdelegateFIFO_T instead
// But You can use pointers of this type
VdelegateFIFO();
VdelegateFIFO(uint16_t size): size(size){
// empty: rx == wx && pushed_objects == 0
// full: rx == wx && pushed_objects > 0
rx = wx = 0;
pushed_objects = 0;
}


// lambda, function or functor object
template<class T>
class Item : public ItemBase{
public:
Item(const T& la): lambda(la){
this->size_ = obect_size();
this->wrapper_ = [](ItemBase* that, TArgs ... args){
static_cast<Item*>(that)->lambda(args...);
};
}

// in sizeof(ItemBase) units
static constexpr size_t obect_size(){
return sizeof(Item)/sizeof(BufferT);
}

void* operator new(size_t, void *ptr){ return ptr; }

private:
T lambda;
};

// -------------------- public --------------------
public:
template<class T>
bool push(const T& obj){
int w = getFreeConsecutiveSpace(Item<T>::obect_size());
if(w<0){ // not enough space
return false;
}
new(&buffer[w]) Item<T>(obj);
wx = w + Item<T>::obect_size();
if(wx >= size)wx = 0; // wrap

pushed_objects++;
return true;
}

ItemBase* getItemToPop(){
if(isEmpty())return nullptr;
ItemBase *item = reinterpret_cast<ItemBase*>(&buffer[rx]);
// wrap if unused tail
if(!item->isUsed()){
rx = 0;
item = reinterpret_cast<ItemBase*>(&buffer[rx]);
}
return item;
}

bool pop(const ItemBase *item){
if(!item)return false;
if(isEmpty())return false;
pushed_objects--;
rx += item->size();
if(rx>=size){
rx=0; // wrap
}else if(rx>wx){
// pop unused tail of buffer too
ItemBase *item = reinterpret_cast<ItemBase*>(&buffer[rx]);
if(!item->isUsed()){
//rx += item->size();
//if(rx >= size)rx=0;
rx=0; // wrap
}
}
return true;
}

bool isEmpty()const{ return pushed_objects==0; }

// ---------------------------------------------------
private:
int getFreeConsecutiveSpace(int sz){
int w = wx;
if(w==rx && pushed_objects>0)return -1; // full
// wx after rx
if(w >= rx){
if(w+sz <= size)return w;
// mark tail of buffer unused
buffer[w] = ItemBase::mark_unused(size-w);
// wrap
w = 0;
}
// wx before rx
if(w+sz <= rx)return w;
return -1;
}

template<class T, int Size> friend class VdelegateFIFO_T;

private:
const uint16_t size; // in sizeof(BufferT) units
uint16_t rx, wx;
uint16_t pushed_objects;
// it's a bit tricky and non-standard but works
BufferT buffer[0];
};

// ------------------ Container ------------------
template<class T, int Size_w>
class VdelegateFIFO_T : public VdelegateFIFO<T>{
public:
VdelegateFIFO_T(): VdelegateFIFO<T>(Size_w) {}

private:
typedef typename VdelegateFIFO<T>::BufferT BufferT;
BufferT buffer[Size_w];
};

template<int Size_w> //, class TBase=void*>
class TaskQueue{
public:
TaskQueue(SST::TPriority priority):
priority(priority)
{
current_task = nullptr; // dont pop first time
}

template<class T>
bool enqueueTask(const T& la){
CriticalSection cs;
cs.enter();
bool e = fifo.isEmpty();
bool r = fifo.push(la);
cs.leave();
if(e)taskCompleted();
return r;
}

bool taskCompleted(){
bool r = SST::postMessage(priority, [this](){
processNextTask();
});
if(!r){
printf("TaskQueue::taskCompleted error: SST_queue %d full\n", priority);
}
return r;
}

private:
void processNextTask(){
CriticalSection cs;
cs.enter();
fifo.pop(current_task); // pop if exists
auto la = fifo.getItemToPop();
current_task = la;
cs.leave();
if(la)la->invoke(); // static_cast<TBase*>(this));
}

private:
typedef VdelegateFIFO_T<void(), Size_w> T_Fifo;
T_Fifo fifo;
typename T_Fifo::ItemBase* current_task;
const SST::TPriority priority;
};