#include <LED_Driver.h>

#define TASK_PRIORITY 20
#define TASK_STACK 2048

namespace drivers
{

    Led::Led()
    {
        LOG_INFO("Inizializing RGB Led");
        pinMode(c_ledPin, OUTPUT);
        m_blinkTask = NULL;
        m_flashTimer = NULL;
        m_enforce = false;
    }

    Led::~Led()
    {
        setColor({0, 0, 0});
        pinMode(c_ledPin, INPUT);
    }

    void Led::setEnforce(const bool enf)
    {
        m_enforce = enf;
    }

    void Led::setColor(const color_t color)
    {
        std::lock_guard<std::mutex> lock(m_ledMutex);
        if (m_enforce)
            return;
        blinkStop();
        m_colorDefault = color;
        rgbLedWrite(c_ledPin, color.g, color.r, color.b);
    }

    void Led::flashHandle(TimerHandle_t th)
    {
        Led *led = (Led *)pvTimerGetTimerID(th);
        rgbLedWrite(led->c_ledPin, led->m_colorDefault.g, led->m_colorDefault.r, led->m_colorDefault.b); // reset color to saved color
        return;
    }

    void Led::flashColor(const uint16_t tOn, const color_t color)
    {
        std::lock_guard<std::mutex> lock(m_ledMutex);
        rgbLedWrite(c_ledPin, color.g, color.r, color.b); // set color to flash
        if (m_flashTimer == NULL)
        {
            m_flashTimer = xTimerCreate("flasher", pdMS_TO_TICKS(tOn), pdFALSE, NULL, flashHandle);
            xTimerStart(m_flashTimer, 0);
            LOG_INFO("Led Flash timer created");
            return;
        }
        xTimerStop(m_flashTimer, 0);
        if (!xTimerChangePeriod(m_flashTimer, pdMS_TO_TICKS(tOn), pdMS_TO_TICKS(1)) || !xTimerReset(m_flashTimer, pdMS_TO_TICKS(1)))
        {
            LOG_ERROR("Led Flash timer failed reset");
            xTimerDelete(m_flashTimer, 0);
            m_flashTimer = NULL;
        }
    }

    void Led::blinkColor(const uint16_t tOn, const uint16_t tOff, const color_t color)
    {
        std::lock_guard<std::mutex> lock(m_ledMutex);
        if (m_enforce)
            return;
        blinkStop();
        m_color1 = color;
        m_color2 = {0, 0, 0};
        m_tOn = tOn;
        m_tOff = tOff;
        xTaskCreate(blinkTask, "blinker", TASK_STACK, this, TASK_PRIORITY, &m_blinkTask);
    }

    void Led::blinkAlternate(const uint16_t tOn, const uint16_t tOff, const color_t color1, const color_t color2)
    {
        std::lock_guard<std::mutex> lock(m_ledMutex);
        if (m_enforce)
            return;
        blinkStop();
        m_color1 = color1;
        m_color2 = color2;
        m_tOn = tOn;
        m_tOff = tOff;
        xTaskCreate(blinkTask, "blinker", TASK_STACK, this, TASK_PRIORITY, &m_blinkTask);
    }

    void Led::blinkStop()
    {
        if (m_blinkTask != NULL)
            vTaskDelete(m_blinkTask);
        m_blinkTask = NULL;
    }

    void Led::blinkTask(void *params)
    {
        Led *led = static_cast<Led *>(params);
        LOG_DEBUG("Blinker Task Created");
        while (true)
        {
            {
                std::lock_guard<std::mutex> lock(led->m_ledMutex);
                rgbLedWrite(led->c_ledPin, led->m_color1.g, led->m_color1.r, led->m_color1.b);
            }
            delay(led->m_tOn);
            {
                std::lock_guard<std::mutex> lock(led->m_ledMutex);
                rgbLedWrite(led->c_ledPin, led->m_color2.g, led->m_color2.r, led->m_color2.b); // off
            }
            if (led->m_tOff == 0)
                break;
            delay(led->m_tOff);
        }
        LOG_DEBUG("Blinker Task Ended");
        led->m_blinkTask = NULL;
        vTaskDelete(NULL);
    }
}