#include "led.h"

// Includes
#include "tools.h"

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

const led_seq_t led_seq_table[] = {

    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  5,  0, 0),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  0,  5, 0),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  0,  0, 5),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  5,  5, 0),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  5,  0, 5),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  0,  5, 5),
    LED_SEQ_MODE_FADE(  1,  0,  0x000000,  10,  5,  5, 5),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0x00FF00, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0xFF0000, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0x0000FF, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0xFFFF00, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0xFF00FF, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0x00FFFF, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 100, ((uint32_t[]){0xFFFFFF, 0x000000, 0x000000, 0x000000, 0x000000}) ),
    LED_SEQ_MODE_BLINK( 1,  0,  0x00FF00, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0xFF0000, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0x0000FF, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0x00FFFF, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0xFFFF00, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0xFF00FF, 100, 0x000000, 500),
    LED_SEQ_MODE_BLINK( 1,  0,  0xFFFFFF, 100, 0x000000, 500),
    //LED_SEQ_MODE_BUFF(  0,  1000,  LED_SEQ_ROT_NONE, 0, ((uint32_t[]){0x00FF00, 0xFF0000, 0x0000FF, 0x00FFFF, 0xFF00FF}) ),
    LED_SEQ_MODE_BUFF(  2,  0,  LED_SEQ_ROT_CW, 1000, ((uint32_t[]){0x00FF00, 0xFF0000, 0x0000FF, 0xFFFF00, 0xFF00FF}) ),

};

led_t led;

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


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


volatile uint32_t is_sent = 0;


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

    // Struct
    led.seq             = &led_seq_table[0];
    led.nseq            = sizeof(led_seq_table) / sizeof(led_seq_table[0]);
    led.iseq            = 0;
    led.seq_step        = 0;
    led.seq_count       = 0;
    led.seq_wait_tick   = 0;
    led.seq_limit       = HAL_GetTick() + led.seq->ms;

    // LED_EN
    gpio.Mode       = GPIO_MODE_OUTPUT_PP;
    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);

    // Period / Prescaler
    // -----------------------
    // 800 kHz * 100 = 80 MHz
    // 800 kHz * 50  = 40 MHz
    // 800 kHz * 40  = 32 MHz
    // 800 kHz * 20  = 16 MHz
    // 800 kHz * 10  =  8 MHz
    // -----------------------
    // 400 kHz * 100 = 40 MHz
    // 400 kHz * 50  = 20 MHz
    // 400 kHz * 40  = 16 MHz
    // 400 kHz * 20  =  8 MHz
    // 400 kHz * 10  =  4 MHz
    // -----------------------

    uint32_t prescaler = HAL_RCC_GetPCLK1Freq() / 8000000;
    led.tim.Instance                = LED_TIM;
    led.tim.Init.Prescaler          = prescaler-1;
    led.tim.Init.CounterMode        = TIM_COUNTERMODE_UP;
    led.tim.Init.Period             = 10-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);




    //led_copy(led.color_buffer, ((uint32_t[]){0x00FF00, 0xFF0000, 0x0000FF, 0x00FFFF, 0xFF00FF}), 5);
    //led_fill(0x0000FF, led.color_buffer, 5);
    //led_send();
    //while(1);




}

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


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;
    }
    //LED_DATA_LOW();
}


void led_wait_busy()
{
    while(led.dma_busy);
}

void led_send()
{
    uint32_t iled, ibit, color;
    uint32_t *cptr = led.color_buffer;
    uint16_t *dptr = led.dma_buffer + 10;
    for(iled = 0; iled < 5; iled++, cptr++){
        color =  ((*cptr << 8) & 0xFF0000) | ((*cptr >> 8) & 0x00FF00) | (*cptr & 0x0000FF);
        for(ibit = 0; ibit < 24; ibit++, dptr++){
            *dptr = ((color & 0x800000) ? 7 : 3);
            color <<= 1;
        }
    }
    led.dma_busy = 1;
    HAL_TIM_PWM_Start_DMA(&led.tim, LED_TIM_CHANNEL, (uint32_t *)led.dma_buffer, LED_DMA_BUFFER_SIZE);

}

void led_task()
{
    // Wait during sequence
    if ((led.seq_wait_tick > 0) && (HAL_GetTick() < led.seq_wait_tick)) return;

    // Next sequence
    if ( ((led.seq->count > 0) && (led.seq_count == led.seq->count)) || ((led.seq->count == 0) && (HAL_GetTick() > led.seq_limit)) ){
        led.iseq++;
        if (led.iseq >= led.nseq) led.iseq = 0;
        led.seq = &led_seq_table[led.iseq];
        led.seq_step = 0;
        led.seq_count = 0;
        led.seq_wait_tick = 0;
        led.seq_limit = HAL_GetTick() + led.seq->ms;
    }

    // Sequence
    switch(led.seq->mode){

        case LED_SEQ_MODE_BLINK:
            if (led.seq_step == 0){
                led.color =led. seq->on_color;
                led.seq_wait_tick = HAL_GetTick() + led.seq->on_ms;
                led.seq_step = 1;
            }else{
                led.color = led.seq->off_color;
                led.seq_wait_tick = HAL_GetTick() + led.seq->off_ms;
                led.seq_step = 0;
                led.seq_count++;
            }
            led_send_reset();
            led_send_npixel(led.color, 5);
            break;

        case LED_SEQ_MODE_FADE:
            if (led.seq_step == 0){
                led.color = led.seq->color;
                led.seq_step = 1;
            }
            led_send_reset();
            led_send_npixel(led.color, 5);
            uint8_t b = (uint8_t)(led.color >> 16);
            uint8_t g = (uint8_t)(led.color >> 8);
            uint8_t r = (uint8_t)led.color;
            if (led.seq_step == 1){
                b += led.seq->inc_b;
                g += led.seq->inc_g;
                r += led.seq->inc_r;
                if ( ((led.seq->inc_b!=0) && (b == 255)) || ((led.seq->inc_g!=0) && (g == 255)) || ((led.seq->inc_r!=0) && (r == 255)) ) {
                    led.seq_step = 2;
                }
            }else if (led.seq_step == 2){
                b -= led.seq->inc_b;
                g -= led.seq->inc_g;
                r -= led.seq->inc_r;
                if ( ((led.seq->inc_b!=0) && (b == 0)) || ((led.seq->inc_g!=0) && (g == 0)) || ((led.seq->inc_r!=0) && (r == 0)) ) {
                    led.seq_step = 1;
                    led.seq_count++;
                }
            }
            led.seq_wait_tick = HAL_GetTick() + led.seq->inc_ms;
            led.color = LED_MK_COLOR(b, g, r);
            break;

        case LED_SEQ_MODE_BUFF:
            if (led.seq_step == 0){
                led_copy(led.buffer, led.seq->ptr, 5);
            }
            led_send_reset();
            led_send_buffer(led.buffer, 5);
            if (led.seq->rot == LED_SEQ_ROT_CW){
                led_rotate_cw(led.buffer, 5);
            }else if (led.seq->rot == LED_SEQ_ROT_CCW){
                led_rotate_ccw(led.buffer, 5);
            }
            if (led.seq->rot != LED_SEQ_ROT_NONE){
                led.seq_wait_tick = HAL_GetTick() + led.seq->rot_ms;
                led.seq_step++;
                if (led.seq_step == 5){
                    led.seq_step = 0;
                    led.seq_count++;
                }
            }
            break;

    }

}


void led_send_reset()
{
    //LED_DATA_LOW();
    HAL_Delay(1);
}

void led_send_npixel(const uint32_t color, uint32_t npx)
{
    led_fill(color, led.color_buffer, 5);
    led_send();
    led_wait_busy();
}

void led_send_buffer(const uint32_t *px, uint32_t npx)
{
    led_copy(led.color_buffer, px, 5);
    led_send();
    led_wait_busy();
}


void led_fill(uint32_t color, uint32_t *px, uint32_t npx)
{
    uint32_t ipx;
    for(ipx = 0; ipx < npx; ipx++, px++){
        *px = color;
    }
}

void led_copy(uint32_t *dest, const uint32_t *src, uint32_t npx)
{
    uint32_t ipx;
    for(ipx = 0; ipx < npx; ipx++, dest++, src++){
        *dest = *src;
    }
}


void led_rotate_cw(uint32_t *px, uint32_t npx)
{
    uint32_t ipx, tmp = px[npx-1];
    for(ipx = npx-1; ipx > 0; ipx--){
        px[ipx] = px[ipx-1];
    }
    px[0] = tmp;
}

void led_rotate_ccw(uint32_t *px, uint32_t npx)
{
    uint32_t ipx, tmp = px[0];
    for(ipx = 0; ipx < npx-1; ipx++){
        px[ipx] = px[ipx+1];
    }
    px[npx-1] = tmp;
}

