#include "led.h"

// Includes
#include <string.h>
#include <stdlib.h> // Rand
#include "tools.h"

// ============================================================================
//                                   GLOBALS
// ============================================================================

led_t led;

// ============================================================================
//                                PROTOTYPES
// ============================================================================


// ============================================================================
//                                 FUNCTION
// ============================================================================


void led_init()
{
    GPIO_InitTypeDef gpio = {0};

    // Init struct
    led.dma_busy    = 0;
    led.brightness  = 50;
    led.led_used    = BUILD_LED_COUNT;
    led_set_seq(LED_SEQ_NONE());

    // LED_EN
    gpio.Mode       = GPIO_MODE_OUTPUT_OD;
    gpio.Pull       = GPIO_NOPULL;
    gpio.Speed      = GPIO_SPEED_FREQ_LOW;
    SYSTEM_GPIO_INIT(gpio, LED_EN_PORT, LED_EN_PIN);
    LED_POWER_ON();

    // LED DATA
    gpio.Mode       = GPIO_MODE_AF_PP;
    gpio.Pull       = GPIO_NOPULL;
    gpio.Speed      = GPIO_SPEED_FREQ_VERY_HIGH;
    gpio.Alternate  = LED_TIM_AF;
    SYSTEM_GPIO_INIT(gpio, LED_DATA_PORT, LED_DATA_PIN);
    LED_DATA_LOW();

    // TIM
    LED_TIM_CLK_ENABLE();
    LED_DMA_CLK_ENABLE();

    led.dma.Instance                    = LED_DMA_CHANNEL;
    led.dma.Init.Request                = LED_DMA_REQUEST;
    led.dma.Init.Direction              = DMA_MEMORY_TO_PERIPH;
    led.dma.Init.PeriphInc              = DMA_PINC_DISABLE;
    led.dma.Init.MemInc                 = DMA_MINC_ENABLE;
    led.dma.Init.PeriphDataAlignment    = DMA_PDATAALIGN_HALFWORD;
    led.dma.Init.MemDataAlignment       = DMA_MDATAALIGN_HALFWORD;
    led.dma.Init.Mode                   = DMA_NORMAL;
    led.dma.Init.Priority               = DMA_PRIORITY_LOW;
    HAL_DMA_Init(&led.dma);

    __HAL_LINKDMA(&led.tim, hdma[TIM_DMA_ID_CC2], led.dma);

    led.tim.Instance                = LED_TIM;
    led.tim.Init.Prescaler          = LED_TIM_PRESCALER;
    led.tim.Init.CounterMode        = TIM_COUNTERMODE_UP;
    led.tim.Init.Period             = LED_TIM_PERIOD - 1;
    led.tim.Init.ClockDivision      = TIM_CLOCKDIVISION_DIV1;
    led.tim.Init.AutoReloadPreload  = TIM_AUTORELOAD_PRELOAD_DISABLE;
    HAL_TIM_PWM_Init(&led.tim);
    TIM_MasterConfigTypeDef sMasterConfig = {0};
    sMasterConfig.MasterOutputTrigger   = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode       = TIM_MASTERSLAVEMODE_DISABLE;
    HAL_TIMEx_MasterConfigSynchronization(&led.tim, &sMasterConfig);
    TIM_OC_InitTypeDef sConfigOC = {0};
    sConfigOC.OCMode                = TIM_OCMODE_PWM1;
    sConfigOC.Pulse                 = 0;
    sConfigOC.OCPolarity            = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastMode            = TIM_OCFAST_DISABLE;
    HAL_TIM_PWM_ConfigChannel(&led.tim, &sConfigOC, LED_TIM_CHANNEL);

    SYSTEM_ENABLE_IRQ(LED_DMA_IRQ, 4, 0);

}


void led_task()
{
    led_color_t color;

    // No sequence
    if (led.seq.head.type == LED_SEQ_TYPE_NONE) return;

    // Wait for next sequence step
    uint32_t now = HAL_GetTick();
    if (led.seq_limit > 0 && now < led.seq_limit) return;

    switch(led.seq.head.type){

        case LED_SEQ_TYPE_BLINK:
            if (led.seq_step == 0){
                led.seq_step = 1;
                led.seq_limit = HAL_GetTick() + led.seq.blink.on_ms;
                color = led.seq.blink.on_color;
                if (led.seq.head.count > 0 && led.seq_count >= led.seq.head.count){
                    led.seq.head.type = LED_SEQ_TYPE_NONE;
                    return;
                }
            }else{
                led.seq_step = 0;
                led.seq_limit = HAL_GetTick() + led.seq.blink.off_ms;
                color = led.seq.blink.off_color;
                led.seq_count++;
            }
            led_set_color(color);
            break;

        case LED_SEQ_TYPE_GLOW:
            {
                int32_t delta;
                uint32_t duration;
                uint32_t elapsed;
                led_color_t c_start, c_end, c_interp;
                // Init timestamp
                if (led.seq_ts == 0) led.seq_ts = now;
                elapsed = now - led.seq_ts;
                // a -> b
                if (led.seq_step == 0){
                    duration = led.seq.glow.a2b_ms;
                    c_start  = led.seq.glow.a_color;
                    c_end    = led.seq.glow.b_color;
                // b -> a
                }else{
                    duration = led.seq.glow.b2a_ms;
                    c_start  = led.seq.glow.b_color;
                    c_end    = led.seq.glow.a_color;
                }

                if (elapsed < duration){
                    delta = (int32_t)c_end.as.r - (int32_t)c_start.as.r;
                    c_interp.as.r = (uint8_t)(c_start.as.r + (delta * (int32_t)elapsed) / (int32_t)duration);
                    delta = (int32_t)c_end.as.g - (int32_t)c_start.as.g;
                    c_interp.as.g = (uint8_t)(c_start.as.g + (delta * (int32_t)elapsed) / (int32_t)duration);
                    delta = (int32_t)c_end.as.b - (int32_t)c_start.as.b;
                    c_interp.as.b = (uint8_t)(c_start.as.b + (delta * (int32_t)elapsed) / (int32_t)duration);
                    led_set_color(c_interp);
                }else{
                    led_set_color(c_end);
                    led.seq_ts = now;
                    led.seq_step = !led.seq_step;
                    if (led.seq_step == 0) {
                        led.seq_count++;
                        if (led.seq.head.count > 0 && led.seq_count >= led.seq.head.count) {
                            led.seq.head.type = LED_SEQ_TYPE_NONE;
                            return;
                        }
                    }
                }

                led.seq_limit = HAL_GetTick() + 20;

            }
            break;

        case LED_SEQ_TYPE_BUFF:
            if (led.seq_limit == 0){
                memcpy(led.led_status, led.seq.buff.colors, sizeof(led_color_t) * BUILD_LED_COUNT);
                led_send();
                led.seq_limit = now + led.seq.buff.ms;
                return;
            }
            if (led.seq.buff.rot == LED_SEQ_BUFF_ROT_NONE) {
                led.seq.head.type = LED_SEQ_TYPE_NONE;
                led.seq_limit = 0;
            }else{
                led_color_t tmp;
                // CW
                if (led.seq.buff.rot == LED_SEQ_BUFF_ROT_CW) {
                    tmp = led.led_status[BUILD_LED_COUNT - 1];
                    memmove(&led.led_status[1], &led.led_status[0], sizeof(led_color_t) * (BUILD_LED_COUNT - 1));
                    led.led_status[0] = tmp;
                // CCW
                }else{
                    tmp = led.led_status[0];
                    memmove(&led.led_status[0], &led.led_status[1], sizeof(led_color_t) * (BUILD_LED_COUNT - 1));
                    led.led_status[BUILD_LED_COUNT - 1] = tmp;
                }
                led_send();

                led.seq_step++;
                if (led.seq_step >= BUILD_LED_COUNT){
                    led.seq_step = 0;
                    led.seq_count++;
                    if (led.seq.head.count > 0 && led.seq_count > led.seq.head.count) {
                        led.seq.head.type = LED_SEQ_TYPE_NONE;
                        return;
                    }
                }

                led.seq_limit = now + led.seq.buff.ms;

            }
            break;

        default:
            break;
    }

}

void led_set_seq(led_seq_t *seq)
{
    size_t sz;
    switch(seq->type){
        case LED_SEQ_TYPE_NONE: sz = sizeof(led_seq_t); break;
        case LED_SEQ_TYPE_BLINK: sz = sizeof(led_seq_blink_t); break;
        case LED_SEQ_TYPE_GLOW: sz = sizeof(led_seq_glow_t); break;
        case LED_SEQ_TYPE_BUFF: sz = sizeof(led_seq_buff_t); break;
        default : return;
    }
    led.seq_count   = 0;
    led.seq_step    = 0;
    led.seq_limit   = 0;
    led.seq_ts      = 0;
    memcpy(&led.seq, seq, sz);
}

void led_send()
{
    int iled, ibit;
    uint8_t g, r, b;

    // Wait busy
    while(led.dma_busy);

    // DMA pointer
    uint16_t *dmaptr = led.dma_buffer;
    led_color_t *colptr = led.led_status;

    // Apply brightness
    uint16_t scale = led.brightness + 1;

    // Loop all led
    for(int iled = 0; iled < led.led_used; iled++) {

        // Apply scale
        g = (colptr->as.g * scale) >> 8;
        r = (colptr->as.r * scale) >> 8;
        b = (colptr->as.b * scale) >> 8;

        // Green
        for (ibit = 7; ibit >= 0; ibit--) {
            *dmaptr++ = (g & (1 << ibit)) ? LED_BIT_1 : LED_BIT_0;
        }

        // Red
        for (ibit = 7; ibit >= 0; ibit--) {
            *dmaptr++ = (r & (1 << ibit)) ? LED_BIT_1 : LED_BIT_0;
        }

        // Blue
        for (ibit = 7; ibit >= 0; ibit--) {
            *dmaptr++ = (b & (1 << ibit)) ? LED_BIT_1 : LED_BIT_0;
        }

        // Next color
        colptr++;
    }

    // Lowering the line after transmission
    *dmaptr++ = 0;

    // Transmit
    led.dma_busy = 1;
    HAL_TIM_PWM_Start_DMA(&led.tim, LED_TIM_CHANNEL, (uint32_t *)led.dma_buffer, LED_DMA_BUFFER_SIZE);
}


// ============================================================================
//                                 TOOLS
// ============================================================================

void led_set_color(led_color_t color)
{
    int ipx;
    led_color_t *px = led.led_status;
    for(ipx = 0; ipx < led.led_used; ipx++, px++){
        *px = color;
    }
    //HAL_Delay(1);
    led_send();
}

led_color_t led_get_rand_color(void)
{
    led_color_t c = {0};
    uint8_t wheel_pos = rand() % 256;

    if(wheel_pos < 85) {
        c.as.r = 255 - wheel_pos * 3;
        c.as.g = 0;
        c.as.b = wheel_pos * 3;
    } else if(wheel_pos < 170) {
        wheel_pos -= 85;
        c.as.r = 0;
        c.as.g = wheel_pos * 3;
        c.as.b = 255 - wheel_pos * 3;
    } else {
        wheel_pos -= 170;
        c.as.r = wheel_pos * 3;
        c.as.g = 255 - wheel_pos * 3;
        c.as.b = 0;
    }
    return c;
}

// ============================================================================
//                                 IRQ HANDLER
// ============================================================================

void LED_DMA_IRQ_HANDLER()
{
    HAL_DMA_IRQHandler(&led.dma);
}

// ============================================================================
//                                 CALLBACK
// ============================================================================

void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == &led.tim){
        HAL_TIM_PWM_Stop_DMA(htim, LED_TIM_CHANNEL);
        led.dma_busy = 0;
    }
}




#if 0
    uint32_t seed = 0;
    // Generate random seed with noise
    ADC_HandleTypeDef adc;
    ADC_ChannelConfTypeDef adccfg = {0};
    __HAL_RCC_ADC_CLK_ENABLE();
    adc.Instance                   = ADC1;
    adc.Init.ClockPrescaler        = ADC_CLOCK_SYNC_PCLK_DIV2; // Horloge synchrone / 2
    adc.Init.Resolution            = ADC_RESOLUTION_12B;       // 12 bits (0-4095)
    adc.Init.DataAlign             = ADC_DATAALIGN_RIGHT;
    adc.Init.ScanConvMode          = ADC_SCAN_DISABLE;         // Un seul canal
    adc.Init.EOCSelection          = ADC_EOC_SINGLE_CONV;
    adc.Init.LowPowerAutoWait      = DISABLE;
    adc.Init.LowPowerAutoPowerOff  = DISABLE;
    adc.Init.ContinuousConvMode    = DISABLE;                  // Conversion sur demande
    adc.Init.NbrOfConversion       = 1;
    adc.Init.DiscontinuousConvMode = DISABLE;
    adc.Init.ExternalTrigConv      = ADC_SOFTWARE_START;       // Déclenchement logiciel
    adc.Init.ExternalTrigConvEdge  = ADC_EXTERNALTRIGCONVEDGE_NONE;
    adc.Init.DMAContinuousRequests = DISABLE;
    adc.Init.Overrun               = ADC_OVR_DATA_PRESERVED;
    adc.Init.SamplingTimeCommon1   = ADC_SAMPLETIME_160CYCLES_5; // Temps long pour accumuler du bruit
    adc.Init.SamplingTimeCommon2   = ADC_SAMPLETIME_160CYCLES_5;
    adc.Init.OversamplingMode      = DISABLE;
    adc.Init.TriggerFrequencyMode  = ADC_TRIGGER_FREQ_HIGH;
    HAL_ADC_Init(&adc);
    adccfg.Channel      = ADC_CHANNEL_TEMPSENSOR;
    adccfg.Rank         = ADC_REGULAR_RANK_1;
    adccfg.SamplingTime = ADC_SAMPLINGTIME_COMMON_1;
    HAL_ADC_ConfigChannel(&adc, &adccfg);
    HAL_ADCEx_Calibration_Start(&adc);
    for (int i = 0; i < 32; i++) {
        HAL_ADC_PollForConversion(&adc, 10);
        uint32_t val = HAL_ADC_GetValue(&adc);
        seed = (seed << 1) | (val & 0x01);
        HAL_Delay(1);
    }
    HAL_ADC_Stop(&adc);
    __HAL_RCC_ADC_CLK_DISABLE();

    // Init seed

    srand(seed);

#endif
