/*
 * @file Worker.cpp
 * @author Robert Borzecki (robert.borzecki@mudita.com)
 * @date 30 maj 2019
 * @brief
 * @copyright Copyright (C) 2019 mudita.com
 * @details
 */
#include <string.h>

extern "C"
{
#include "FreeRTOS.h"
#include "task.h"
}

// module-sys
#include "Worker.hpp"

namespace sys
{

    void workerTaskFunction(void *ptr)
    {
        Worker *worker = reinterpret_cast<Worker *>(ptr);
        QueueSetMemberHandle_t activeMember;
        std::vector<xQueueHandle> queues = worker->queues;

        while (1) {
            activeMember = xQueueSelectFromSet(worker->queueSet, portMAX_DELAY);
            // find id of the queue that was activated

            for (uint32_t i = 0; i < queues.size(); i++) {
                if (queues[i] == activeMember) {
                    worker->handleMessage(i);
                }
            }
        }
    }

    Worker::Worker(sys::Service *service) : service{service}, serviceQueue{NULL}, queueSet{NULL}, taskHandle{NULL}
    {}

    Worker::~Worker()
    {}

    bool Worker::init(std::list<WorkerQueueInfo> queuesList)
    {

        // initial value is because there is always a queue to communicate with service
        uint32_t setSize = SERVICE_QUEUE_LENGTH;

        auto addQueueInfo = [&](xQueueHandle q, std::string qName) {
            queueNameMap.insert(std::pair<xQueueHandle, std::string>(q, qName));
            vQueueAddToRegistry(q, qName.c_str());
            queues.push_back(q);
        };

        // iterate over all entries in the list of queues and summarize queue sizes
        for (auto wqi : queuesList) {
            setSize += wqi.length;
        }

        // create set of queues
        queueSet = xQueueCreateSet(setSize);
        if (queueSet == NULL)
            return false;

        // create and add all queues to the set. First service queue is created.
        serviceQueue = xQueueCreate(SERVICE_QUEUE_LENGTH, SERVICE_QUEUE_SIZE);
        if (serviceQueue == nullptr) {
            deinit();
            return false;
        }

        addQueueInfo(serviceQueue, SERVICE_QUEUE_NAME);

        // create and add all queues provided from service
        for (auto wqi : queuesList) {
            auto q = xQueueCreate(wqi.length, wqi.elementSize);
            if (q == NULL) {
                LOG_FATAL("xQueueCreate %s failed", wqi.name.c_str());
                deinit();
                return false;
            }
            addQueueInfo(q, wqi.name);
        };

        // iterate over all queues and add them to set
        for (uint32_t i = 0; i < queues.size(); ++i) {

            if (xQueueAddToSet(queues[i], queueSet) != pdPASS) {
                //			LOG_FATAL("xQueueAddToSet %d failed", i);
                deinit();
                return false;
            }
        }

        return true;
    }
    bool Worker::deinit()
    {

        vTaskDelete(taskHandle);

        // for all queues - remove from set and delete queue
        for (auto q : queues) {
            // remove queues from set
            xQueueRemoveFromSet(q, queueSet);
            // delete queue
            vQueueDelete(q);
        }
        queues.clear();

        // delete queues set
        vQueueDelete((QueueHandle_t)queueSet);
        queueSet   = NULL;
        taskHandle = NULL;

        return true;
    };
    /**
     * This method starts RTOS thread that waits for incomming queue events.
     */
    bool Worker::run()
    {
        static int workerCount = 0;
        std::string workerName = service->GetName() + "_w" + std::to_string(workerCount);
        BaseType_t task_error =
            xTaskCreate(workerTaskFunction, workerName.c_str(), 2048, this, service->GetPriority(), &taskHandle);
        if (task_error != pdPASS) {
            LOG_ERROR("Failed to start the task");
            return false;
        }
        return true;
    }

    bool Worker::stop()
    {
        return send(0, NULL);
    }

    bool Worker::handleMessage(uint32_t queueID)
    {

        QueueHandle_t queue = queues[queueID];

        // service queue
        if (queueID == 0) {
            WorkerCommand wcmd;
            if (xQueueReceive(queue, &wcmd, 0) != pdTRUE) {
                return false;
            }
            wcmd.command = 1;
            // place some code here to handle messages from service
        }
        return true;
    }

    bool Worker::send(uint32_t cmd, uint32_t *data)
    {
        if (serviceQueue != NULL) {
            WorkerCommand wcmd{cmd, data};
            if (xQueueSend(serviceQueue, &wcmd, portMAX_DELAY) == pdTRUE)
                return true;
        }
        return false;
    }

    xQueueHandle Worker::getQueueByName(std::string qname)
    {
        for (auto q_handle : this->queues) {
            if (this->queueNameMap[q_handle] == qname)
                return q_handle;
        }
        return nullptr;
    }

} /* namespace sys */