/* 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"

uint16_t adc_data[192];
bool sync_running = false;

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 = 1;
    TIM1->ARR = 32767;
    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);

    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 = 124; /* 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;

    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_TOVS | ADC_CFGR2_OVSE; /* Oversample by 16 */
    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;
    GPIOB->BSRR = (1<<7);
    if (!sync_running) {
        while(DMA1_Channel1->CCR != 0) {
            DMA1_Channel1->CCR = 0;
        }
        DMA1_Channel1->CCR = (1<<DMA_CCR_MSIZE_Pos) | (1<<DMA_CCR_PSIZE_Pos) | DMA_CCR_MINC | DMA_CCR_TCIE;
        DMA1_Channel1->CNDTR = COUNT_OF(adc_data);
        DMA1_Channel1->CCR |= DMA_CCR_EN;
        sync_running = true;
    }
}

void TIM1_CC_IRQHandler(void) {
    TIM1->SR &= ~TIM_SR_CC1IF;
    GPIOB->BRR = (1<<7);
}

void DMA1_Channel1_IRQHandler(void) {
    static int32_t bottom = -1;
    static int32_t top = -1;

    DMA1->IFCR = DMA_IFCR_CTCIF1;

    if (bottom >= 0) {
        uint32_t amplitude = top - bottom;
        uint32_t middle = bottom + amplitude / 2;

        const uint32_t lower_thr = bottom + amplitude / 4;
        const uint32_t upper_thr = top - amplitude / 4;
        const uint32_t adc_clockdiv = 125 * 32;
        //const uint32_t adc_clockdiv = 1; /* FIXME DEBUG */

        size_t num_edges = 0;
        ssize_t edge_indices[24];

        int state = 0;
        ssize_t run_start = -1;
        int last_v = -1;
        for (ssize_t i=0; i<COUNT_OF(adc_data)-1; i++) {
            uint32_t a = adc_data[i], b = adc_data[i+1];

            if (state == 0) {
                if (a < lower_thr && b > lower_thr) {
                    state = 1;
                    run_start = i+1;
                } else if (a > upper_thr && b < upper_thr) {
                    state = -1;
                    run_start = i+1;
                }

            } else if (state == 1) {
                if (b < a) {
                    state = 0;
                } else if (a < upper_thr && b > upper_thr) {
                    /* run from run_start (incl.) to i (incl.) */
                    uint32_t v0 = adc_data[run_start];
                    int d = a - v0;
                    int c = i - run_start;
                    size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d;
                    if (num_edges < COUNT_OF(edge_indices)) {
                        edge_indices[num_edges] = intercept;
                        num_edges++;
                    }
                    state = 0;
                }

            } else if (state == -1) {
                if (b > a) {
                    state = 0;
                } else if (a > lower_thr && b < lower_thr) {
                    /* run from run_start (incl.) to i (incl.) */
                    uint32_t v0 = adc_data[run_start];
                    int d = a - v0;
                    int c = i - run_start;
                    size_t intercept = run_start * adc_clockdiv + (middle - v0) * adc_clockdiv * c / d;
                    if (num_edges < COUNT_OF(edge_indices)) {
                        edge_indices[num_edges] = intercept;
                        num_edges++;
                    }
                    state = 0;
                }
            }
        }

        asm volatile ("bkpt");
    }

    const int discard = 5;
    const int keep = 32;

    quicksort(adc_data, &adc_data[COUNT_OF(adc_data)-1]);
    for (size_t i=0; i<keep; i++) {
        bottom += adc_data[discard + i];
        top += adc_data[COUNT_OF(adc_data) - 1 - discard - i];
    }

    bottom /= keep;
    top /= keep;
    sync_running = false;
}

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;
        }
    }
}