#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_lp.pin = c_ledPin;
        m_lp.blinkTask = NULL;
    }

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

    void Led::setColor(const color_t color)
    {
        blinkStop();
        rgbLedWrite(c_ledPin, color.r, color.g, color.b);
    }

    void Led::blinkColor(const uint16_t tOn, const uint16_t tOff, const color_t color)
    {
        blinkStop();
        m_lp.color1 = color;
        m_lp.color2 = {0, 0, 0};
        m_lp.tOn = tOn;
        m_lp.tOff = tOff;
        xTaskCreate(blinkTask, "blinker", TASK_STACK, static_cast<void *>(&m_lp), TASK_PRIORITY, &m_lp.blinkTask);
    }

    void Led::blinkAlternate(const uint16_t tOn, const uint16_t tOff, const color_t color1, const color_t color2)
    {
        {
            blinkStop();
            m_lp.color1 = color1;
            m_lp.color2 = color2;
            m_lp.tOn = tOn;
            m_lp.tOff = tOff;
            xTaskCreate(blinkTask, "blinker", TASK_STACK, static_cast<void *>(&m_lp), TASK_PRIORITY, &m_lp.blinkTask);
        }
    }

    void Led::blinkStop()
    {
        if (m_lp.blinkTask != NULL)
            vTaskDelete(m_lp.blinkTask);
        m_lp.blinkTask = NULL;
    }

    void Led::blinkTask(void *params)
    {
        LOG_DEBUG("Blinker Task Created");
        led_params_t *lPar = static_cast<led_params_t *>(params);
        while (true)
        {
            rgbLedWrite(lPar->pin, lPar->color1.g, lPar->color1.r, lPar->color1.b);
            delay(lPar->tOn);
            rgbLedWrite(lPar->pin, lPar->color2.g, lPar->color2.r, lPar->color2.b); // off
            if (lPar->tOff == 0)
                break;
            delay(lPar->tOff);
        }
        LOG_DEBUG("Blinker Task Ended");
        lPar->blinkTask = NULL;
        vTaskDelete(NULL);
    }
}