#include "leds.h"
#include <hal.h>
#include <ch.h>
#include <stm32l1xx.h>

#define MAX_PWM_DIV 65536
#define MIN_PWM_DIV 1024

void leds_init()
{
    RCC->APB2ENR |= RCC_APB2ENR_TIM9EN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM10EN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
    
    TIM9->PSC = TIM10->PSC = TIM11->PSC = 0;
    TIM9->ARR = TIM10->ARR = TIM11->ARR = MIN_PWM_DIV - 1;
    TIM9->CCMR1 = TIM10->CCMR1 = TIM11->CCMR1 = 0x68;
    TIM9->CCER = TIM10->CCER = TIM11->CCER = 1;
    TIM9->CCR1 = TIM10->CCR1 = TIM11->CCR1 = 0;
    TIM9->CNT = TIM10->CNT = TIM11->CNT = 0;
    
    TIM9->CR1 = TIM10->CR1 = TIM11->CR1 = 0x81; // Enable ARR preload, start timer
}

// Convert logarithmic brightness value to PWM value
// (Gamma correction with power 2)
// Selects the divider (ARR) so that it is smallest at high brightness,
// to reduce flicker, and larger at low brightness, to increase resolution.
static void val_to_pwm(int value, int *n, int *d)
{
    int64_t abs_brightness = value * value;
    int64_t div = LED_MAX * LED_MAX;
    
    // Choose best divider
    if (abs_brightness != 0 && abs_brightness * MIN_PWM_DIV / div < 50)
    {
        int64_t tmp = 50 * div / abs_brightness;
        if (tmp > MAX_PWM_DIV)
            tmp = MAX_PWM_DIV;
        
        *d = tmp;
    }
    else
    {
        *d = MIN_PWM_DIV;
    }
    
    *n = (abs_brightness * (*d) + div / 2) / div;
}

static int g_red, g_green, g_blue;

void set_leds(int red, int green, int blue)
{
    if (red < 0) red = 0; if (red > LED_MAX) red = LED_MAX;
    if (green < 0) green = 0; if (green > LED_MAX) green = LED_MAX;
    if (blue < 0) blue = 0; if (blue > LED_MAX) blue = LED_MAX;
    g_red = red; g_green = green; g_blue = blue;
    
    int rn, rd, gn, gd, bn, bd;
    val_to_pwm(red, &rn, &rd);
    val_to_pwm(green, &gn, &gd);
    val_to_pwm(blue, &bn, &bd);
    
    // Disable updates while we set the registers
    TIM9->CR1 |= TIM_CR1_UDIS;
    TIM10->CR1 |= TIM_CR1_UDIS;
    TIM11->CR1 |= TIM_CR1_UDIS;
    
    TIM9->CCR1 = rn; TIM9->ARR = rd - 1;
    TIM10->CCR1 = gn; TIM10->ARR = gd - 1;
    TIM11->CCR1 = bn; TIM11->ARR = bd - 1;
    
    // Let all the timers update on their next overflow
    TIM9->CR1 &= ~TIM_CR1_UDIS;
    TIM10->CR1 &= ~TIM_CR1_UDIS;
    TIM11->CR1 &= ~TIM_CR1_UDIS;
}

void get_leds(int *red, int *green, int *blue)
{
    *red = g_red;
    *green = g_green;
    *blue = g_blue;
}

void fade_leds(int r2, int g2, int b2, int time)
{
    int r1, g1, b1;
    get_leds(&r1, &g1, &b1);
    
    for (int i = 0; i <= time; i++)
    {
        int r, g, b;
        
        r = i * (r2 - r1) / time + r1;
        g = i * (g2 - g1) / time + g1;
        b = i * (b2 - b1) / time + b1;
        
        set_leds(r, g, b);
        chThdSleepMilliseconds(1);
    }
}