/* Megumin LED display firmware
 * Copyright (C) 2018 Sebastian Götte <code@jaseg.net>
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "global.h"
#include "math.h"
#include "color.h"

static struct hsvf g_color_s = {0.0f, 0.0f, 0.0f};
struct hsvf *g_color = &g_color_s;
volatile uint64_t g_time_ms = 0;

uint64_t wait_until(uint64_t timestamp);
bool check_interval(uint64_t *timestamp, uint64_t interval);
void blink_led(uint64_t *ts, int t_on, int t_off, GPIO_TypeDef *gpio, int pin);

void update_timers(struct rgbf *rgb);

uint32_t pcg32_random_r() {
    // *Really* minimal PCG32 code / (c) 2014 M.E. O'Neill / pcg-random.org
    // Licensed under Apache License 2.0 (NO WARRANTY, etc. see website)
    static uint64_t state = 0xbc422715d3aef60f;
    static uint64_t inc = 0x6605e3bc6d1a869b; 
    uint64_t oldstate = state;
    // Advance internal state
    state = oldstate * 6364136223846793005ULL + (inc|1);
    // Calculate output function (XSH RR), uses old state for max ILP
    uint32_t xorshifted = ((oldstate >> 18u) ^ oldstate) >> 27u;
    uint32_t rot = oldstate >> 59u;
    return (xorshifted >> rot) | (xorshifted << ((-rot) & 31));
}

int main(void){
    /* We're starting out from HSI@8MHz */
    SystemCoreClockUpdate();
    SysTick_Config(SystemCoreClock / 1000); /* 1ms tick */

    /* Turn on lots of neat things */
    RCC->APB2ENR |= RCC_APB2ENR_IOPAEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPCEN | RCC_APB2ENR_AFIOEN | RCC_APB2ENR_TIM1EN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM4EN;

    GPIOC->CRH |=
          (0<<GPIO_CRH_CNF13_Pos) | (2<<GPIO_CRH_MODE13_Pos); /* PC13 - LED */
    GPIOB->CRL |=
          (2<<GPIO_CRL_CNF5_Pos) | (2<<GPIO_CRL_MODE5_Pos); /* PB5 - TIM3_CH2 (r fractional) */
    AFIO->MAPR |= (2 << AFIO_MAPR_TIM3_REMAP_Pos); /* Map TIM3_CH2 to PB5 */
    GPIOB->CRH |=
          (2<<GPIO_CRH_CNF8_Pos) | (2<<GPIO_CRH_MODE8_Pos)  /* PB8 - TIM4_CH3 (g fractional) */
        | (2<<GPIO_CRH_CNF9_Pos) | (2<<GPIO_CRH_MODE9_Pos); /* PB9 - TIM4_CH4 (b fractional) */
    GPIOC->ODR |= 1<<13; /* LED */

    GPIOA->CRH = (2<<GPIO_CRH_CNF8_Pos) | (2<<GPIO_CRH_MODE8_Pos); /* PA8 - TIM1_CH1 (global dimming) */


    TIM3->SMCR = (3<<TIM_SMCR_TS_Pos) | (4 << TIM_SMCR_SMS_Pos);
    TIM4->CR2 = (4<<TIM_CR2_MMS_Pos);

    TIM3->CCER = TIM_CCER_CC2E;
    TIM3->CCMR1 = (0<<TIM_CCMR1_CC2S_Pos) | TIM_CCMR1_OC2PE | (6<<TIM_CCMR1_OC2M_Pos);
    TIM4->CCER = TIM_CCER_CC3E | TIM_CCER_CC4E | TIM_CCER_CC1E | TIM_CCER_CC3P | TIM_CCER_CC4P;
    TIM4->CCMR1 = (0<<TIM_CCMR1_CC1S_Pos) | TIM_CCMR1_OC1PE | (7<<TIM_CCMR1_OC1M_Pos);
    TIM4->CCMR2 = (0<<TIM_CCMR2_CC4S_Pos) | TIM_CCMR2_OC4PE | (6<<TIM_CCMR2_OC4M_Pos) \
                | (0<<TIM_CCMR2_CC3S_Pos) | TIM_CCMR2_OC3PE | (7<<TIM_CCMR2_OC3M_Pos);

    TIM3->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;
    TIM4->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;
    TIM4->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;

    TIM1->CCMR1 = (0<<TIM_CCMR1_CC1S_Pos) | TIM_CCMR1_OC1PE | (6<<TIM_CCMR1_OC1M_Pos);
    TIM1->SMCR = (3 << TIM_SMCR_TS_Pos) | (4 << TIM_SMCR_SMS_Pos);
    TIM1->CCER = TIM_CCER_CC1E;
    TIM1->ARR = 0xffff;
    TIM1->BDTR = TIM_BDTR_MOE;
    TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;

    uint64_t ts = g_time_ms;
    uint64_t led_ts = 0;
    for (;;) {
        g_color->h = fmodf(g_color->h + 0.0005, 1.0f);
        //g_color->h = 0.0;
        g_color->s = 0.8;
        g_color->v = 1.0;

        struct rgbf rgb;
        hsv_to_rgb(g_color, &rgb);
        update_timers(&rgb);

        ts = wait_until(ts + 5);

        blink_led(&led_ts, 100, 200, GPIOC, 13);
    }
}

void update_timers(struct rgbf *rgb) {
    float gamma = 2.2f;
    rgb->r = powf(rgb->r, gamma);
    rgb->g = powf(rgb->g, gamma);
    rgb->b = powf(rgb->b, gamma);

    float total = rgb->r + rgb->g + rgb->b;
    rgb->r /= total;
    rgb->g /= total;
    rgb->b /= total;

    float period = 0xffff;
    int thr[2] = {roundf(period * rgb->r), roundf(period * (rgb->r + rgb->g))};

    TIM4->CCR3 = thr[0];
    TIM4->CCR4 = thr[1];
    TIM4->CCR1 = thr[0];
    TIM3->CCR2 = thr[1] - thr[0];
    TIM3->ARR  = period;
    TIM4->ARR  = period;

    TIM1->CCR1 = roundf((period - 1) * total / 3.0f);
}

void gdb_dump(void) {
    /* debugger hook */
}

void NMI_Handler(void) {
    asm volatile ("bkpt");
}

void HardFault_Handler(void) __attribute__((naked));
void HardFault_Handler() {
    asm volatile ("bkpt");
}

void SVC_Handler(void) {
    asm volatile ("bkpt");
}


void PendSV_Handler(void) {
    asm volatile ("bkpt");
}

void SysTick_Handler(void) {
    g_time_ms ++;
}

uint64_t wait_until(uint64_t timestamp) {
    while (g_time_ms < timestamp)
        ;
    return g_time_ms;
}

bool check_interval(uint64_t *timestamp, uint64_t interval) {
    if (*timestamp == 0) {
        *timestamp = g_time_ms;
        return false;
    }

    if (g_time_ms < *timestamp + interval)
        return false;

    *timestamp = g_time_ms;
    return true;
}

void blink_led(uint64_t *ts, int t_on, int t_off, GPIO_TypeDef *gpio, int pin) {
    int bm = 1<<pin;
    if (gpio->ODR & bm) {
        if (check_interval(ts, t_off))
            gpio->BRR = bm;

    } else {
        if (check_interval(ts, t_on))
            gpio->BSRR = bm;
    }
}