/*
 * rtos_wrap.hpp
 *
 *  Created on: 13 feb 2019
 *      Author: MatteoRagni
 */

#ifndef APPLICATION_USER_CORE_RTOS_WRAP_HPP_
#define APPLICATION_USER_CORE_RTOS_WRAP_HPP_

#include <map>
#include <typeindex>
#include <typeinfo>
#include <stm32f7xx_it.h>
#include <FreeRTOS.h>
#include <queue.h>

#define AM_QUEUE_DEBUG
#ifdef AM_QUEUE_DEBUG
#define AM_QUEUE_ASSERT(cond) { if(!(cond)) HardFault_Handler();  }
#else
#define AM_QUEUE_ASSERT(conf) (void(0))
#endif


namespace am {

  class Queue {

    static std::map<size_t, Queue*> queues;
    const size_t local_size;
    size_t local_length;
    const std::type_index local_type;

    QueueHandle_t local_queue;

    Queue(const size_t length, const size_t size, const std::type_index type) :
      local_size(size),
      local_length(0),
      local_type(type),
      local_queue(NULL) {

      resize(length);
      local_length = length; // must be done after the first resize
      Queue::queues[local_type.hash_code()] = this;
    }

  public:
    ~Queue() {
      if (local_queue)
        vQueueDelete(local_queue);
      auto q = Queue::queues.find(local_type.hash_code());
      if (q != Queue::queues.end())
        Queue::queues.erase(q);
    }


    template <typename payload_t>
    static Queue* queue(const size_t length = 1) {
      auto q = Queue::queues.find(typeid(payload_t).hash_code());
      if (q != Queue::queues.end()) {
        Queue * cq = q->second;
        cq->resize(length);
        return q->second;
      } else {
        return new Queue(length, sizeof(payload_t), typeid(payload_t));
      }
    }

    template <typename payload_t>
    bool put(payload_t * data, const size_t wait_ms = 0) const {
      AM_QUEUE_ASSERT(typeid(payload_t).hash_code() == local_type.hash_code());
      const TickType_t tick = wait_ms / portTICK_PERIOD_MS;
      return (xQueueSend(local_queue, (void *)data, tick) == pdTRUE ? true : false);
    }

    template <typename payload_t>
    bool overwrite(payload_t * data, const size_t wait_ms = 0) const {
      AM_QUEUE_ASSERT(typeid(payload_t).hash_code() == local_type.hash_code());
      AM_QUEUE_ASSERT(local_length == 1);
      const TickType_t tick = wait_ms / portTICK_PERIOD_MS;
      xQueueOverwrite(local_queue, (void *)data);
      return true; // xQueueOverwrite can only return true
    }

    template <typename payload_t>
    bool get(payload_t * data, const size_t wait_ms = 0) const {
      AM_QUEUE_ASSERT(typeid(payload_t).hash_code() == local_type.hash_code());
      const TickType_t tick = wait_ms / portTICK_PERIOD_MS;
      return (xQueueReceive(local_queue, (void *)data, tick) == pdTRUE ? true : false);
    }

    template <typename payload_t>
    bool peek(payload_t * data, const size_t wait_ms = 0) const {
      AM_QUEUE_ASSERT(typeid(payload_t).hash_code() == local_type.hash_code());
      const TickType_t tick = wait_ms / portTICK_PERIOD_MS;
      return (xQueuePeek(local_queue, (void *)data, tick) == pdTRUE ? true : false);
    }

    size_t waiting() const {
      return (size_t)uxQueueMessagesWaiting(local_queue);
    }

    void clear() const {
      xQueueReset(local_queue);
    }

    void resize(const size_t length) {
      if (length != local_length) {
        if (local_queue) {
          vQueueDelete(local_queue);
        }
        local_queue = xQueueCreate(length, local_size);
        if (!local_queue) {
          MemManage_Handler();
        }
      }
    }




  };

}

#endif /* APPLICATION_USER_CORE_RTOS_WRAP_HPP_ */