/* 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 "8b10b.h"

static uint16_t adc_data[64*2];
static volatile struct state_8b10b_dec st_8b10b_dec;

static void quicksort(uint16_t *head, uint16_t *tail);

int main(void) {
    /* Configure clocks for 64 MHz system clock.
     * 
     * HSI @ 16 MHz --[PLL x16 /4]--> PLL "R" clock @ 64 MHz
     */
    /* Enable peripherals */
    RCC->APBENR1 |= RCC_APBENR1_PWREN;
    /* Increase flash wait states to 2 required for operation above 48 MHz */
    FLASH->ACR = FLASH_ACR_ICEN | FLASH_ACR_PRFTEN | (FLASH->ACR & ~FLASH_ACR_LATENCY_Msk) | (2<<FLASH_ACR_LATENCY_Pos);
    while ((FLASH->ACR & FLASH_ACR_LATENCY_Msk) != (2<<FLASH_ACR_LATENCY_Pos)) {
        /* wait for flash controller to acknowledge change. */
    }
    /* Configure PLL with multiplier 16, divisor 2 for "R" output, and enable "R" (sysclk) output */
    RCC->PLLCFGR = (16<<RCC_PLLCFGR_PLLN_Pos) | (2<<RCC_PLLCFGR_PLLSRC_Pos) | (3<<RCC_PLLCFGR_PLLR_Pos) | RCC_PLLCFGR_PLLREN;
    RCC->CR |= RCC_CR_PLLON;
    while (!(RCC->CR & RCC_CR_PLLRDY)) {
        /* wait for PLL to stabilize. */
    }
    /* Switch SYSCLK to PLL source. */
    RCC->CFGR |= (2<<RCC_CFGR_SW_Pos);
    while ((RCC->CFGR & RCC_CFGR_SWS_Msk) != (2<<RCC_CFGR_SWS_Pos)) {
        /* wait for RCC to switch over. */
    }

    RCC->AHBENR |= RCC_AHBENR_DMA1EN;
    RCC->APBENR1 |= RCC_APBENR1_TIM3EN | RCC_APBENR1_DBGEN;
    RCC->APBENR2 |= RCC_APBENR2_TIM1EN | RCC_APBENR2_ADCEN;
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN | RCC_IOPENR_GPIOCEN;

    /*
    TIM1->PSC = 0;
    TIM1->ARR = nominal_period;
    TIM1->DIER = TIM_DIER_UIE | TIM_DIER_CC1IE;
    TIM1->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;
    TIM1->CCR1 = 3000;
    NVIC_EnableIRQ(TIM1_BRK_UP_TRG_COM_IRQn);
    NVIC_SetPriority(TIM1_BRK_UP_TRG_COM_IRQn, 0);
    NVIC_EnableIRQ(TIM1_CC_IRQn);
    NVIC_SetPriority(TIM1_CC_IRQn, 0);
    */

    xfr_8b10b_reset((struct state_8b10b_dec *)&st_8b10b_dec);

    TIM3->CR1 = TIM_CR1_ARPE;
    TIM3->CR2 = (2<<TIM_CR2_MMS_Pos); /* Update event on TRGO */
    TIM3->PSC = 0;
    /* We sample 32 times per 1 kHz AC cycle, and use 32 times oversampling. */
    TIM3->ARR = 125*16; /* Output 64 MHz / 125 = 512 kHz signal */
    TIM3->CR1 |= TIM_CR1_CEN;
    
    DMAMUX1[0].CCR = 5; /* ADC */
    DMA1_Channel1->CPAR = (uint32_t)&ADC1->DR;
    DMA1_Channel1->CMAR = (uint32_t)(void *)adc_data;
    DMA1_Channel1->CNDTR = COUNT_OF(adc_data);
    DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_CIRC | DMA_CCR_HTIE | DMA_CCR_TCIE;
    DMA1_Channel1->CCR |= DMA_CCR_EN;

    NVIC_EnableIRQ(DMA1_Channel1_IRQn);
    NVIC_SetPriority(DMA1_Channel1_IRQn, 64);

    ADC1->ISR = ADC_ISR_CCRDY | ADC_ISR_ADRDY; /* Clear CCRDY */
    ADC1->CR = ADC_CR_ADVREGEN;
    delay_us(20);
    ADC1->CR = ADC_CR_ADCAL;
    while (ADC1->CR & ADC_CR_ADCAL) {
        /* wait. */
    }
    ADC1->CFGR1 = (1<<ADC_CFGR1_EXTEN_Pos) | (3<<ADC_CFGR1_EXTSEL_Pos) | ADC_CFGR1_DMAEN | ADC_CFGR1_DMACFG; /* TIM3 TRGO */
    ADC1->CFGR2 = (1<<ADC_CFGR2_CKMODE_Pos) | (4<<ADC_CFGR2_OVSR_Pos) | (1<<ADC_CFGR2_OVSS_Pos) | ADC_CFGR2_OVSE;
    ADC1->CHSELR = (1<<4); /* Enable input 4 -> PA4 (Vdiff)*/
    while (!(ADC1->ISR & ADC_ISR_CCRDY)) {
        /* wait. */
    }
    ADC1->ISR = ADC_ISR_CCRDY; /* Clear CCRDY */
    ADC->CCR = ADC_CCR_TSEN | ADC_CCR_VREFEN;
    ADC1->CR = ADC_CR_ADVREGEN | ADC_CR_ADEN;
    while (!(ADC1->ISR & ADC_ISR_ADRDY)) {
        /* wait. */
    }
    ADC1->CR |= ADC_CR_ADSTART;

    GPIOA->MODER = OUT(0) | IN(1) | OUT(2) | OUT(3) | ANALOG(4) | OUT(5) | OUT(6) | IN(7) | ANALOG(9) | ANALOG(10) | OUT(11) | ANALOG(12)| AF(13) | AF(14);
    GPIOB->MODER = ANALOG(0) | OUT(3) | ANALOG(1) | ANALOG(2) | ANALOG(4) | ANALOG(5) | ANALOG(6) | ANALOG(8) | OUT(7) | ANALOG(9);
    GPIOC->MODER = OUT(15) | ANALOG(14) | ANALOG(9);

    DBG->APBFZ1 |= DBG_APB_FZ1_DBG_TIM3_STOP;
    DBG->APBFZ2 |= DBG_APB_FZ2_DBG_TIM1_STOP;
    while (42) {
    }
}

/*
void TIM1_BRK_UP_TRG_COM_IRQHandler(void) {
    TIM1->SR &= ~TIM_SR_UIF;
}

void TIM1_CC_IRQHandler(void) {
    TIM1->SR &= ~TIM_SR_CC1IF;
}
*/
static size_t received_symbols = 0;
static int symbol_buf[32];
static size_t received_bits = 0;
static int16_t bit_buf[256];
size_t adc_reduced_pos = 0;
static uint8_t adc_reduced[4096];

void DMA1_Channel1_IRQHandler(void) {
    static int sampling_phase = 0;
    static int last_sample = 0;

    uint16_t *buf = (DMA1->ISR & DMA_ISR_HTIF1) ? &adc_data[0] : &adc_data[COUNT_OF(adc_data)/2];
    DMA1->IFCR = DMA_IFCR_CGIF1;
    GPIOB->BSRR = (1<<7);

    const int threshold_adc_counts = 28500;
    const int sample_per_baud = 16;

    for (size_t i=0; i<COUNT_OF(adc_data)/2; i++) {
        int sample = buf[i];

        adc_reduced[adc_reduced_pos] = (sample & 0xffff)>>9;
        if (adc_reduced_pos == 0) {
            asm volatile ("bkpt");
        }

        if ((last_sample <= threshold_adc_counts && sample >= threshold_adc_counts) ||
            (last_sample >= threshold_adc_counts && sample <= threshold_adc_counts)){
            sampling_phase = sample_per_baud / 4; /* /2 for half baud sampling point, /2 for sinusoidal edge shape */

        } else if (sampling_phase == 0) {
            int bit = sample < threshold_adc_counts;
            adc_reduced[adc_reduced_pos] |= 0x80;

            bit_buf[received_bits] = bit;
            received_bits = (received_bits+1) % COUNT_OF(bit_buf);

            int rc =  xfr_8b10b_feed_bit((struct state_8b10b_dec *)&st_8b10b_dec, bit);
            if (rc > -K_CODES_LAST) {
                symbol_buf[received_symbols] = rc;
                received_symbols = (received_symbols+1) % COUNT_OF(symbol_buf);
            }
            sampling_phase = sample_per_baud;

        } else {
            sampling_phase--;
        }

        adc_reduced_pos = (adc_reduced_pos+1) % COUNT_OF(adc_reduced);
        last_sample = sample;
    }

    GPIOB->BRR = (1<<7);
}

void delay_us(int duration_us) {
    while (duration_us--) {
        for (int i=0; i<32; i++) {
            asm volatile ("nop");
        }
    }
}

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 __libc_init_array (void) __attribute__((weak));
void __libc_init_array () {
}

/* https://github.com/openmv/openmv/blob/2e8d5d505dbe695b8009d832e5ef7691009148e1/src/omv/common/array.c#L117 */
static void quicksort(uint16_t *head, uint16_t *tail) {
    while (head < tail) {
        uint16_t *h = head - 1;
        uint16_t *t = tail;
        uint16_t v = tail[0];
        for (;;) {
            do {
                ++h;
            } while (h < t && h[0] < v);
            do {
                --t;
            } while (h < t && v < t[0]);
            if (h >= t) {
                break;
            }
            uint16_t x = h[0];
            h[0] = t[0];
            t[0] = x;
        }
        uint16_t x = h[0];
        h[0] = tail[0];
        tail[0] = x;
        // do the smaller recursive call first, to keep stack within O(log(N))
        if (t - head < tail - h - 1) {
            quicksort(head, t);
            head = h + 1;
        } else {
            quicksort(h + 1, tail);
            tail = t;
        }
    }
}