/* 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*2];

const int nominal_period = 125*16*32/2;

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);
    */

    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*32; /* 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;
}
*/

void DMA1_Channel1_IRQHandler(void) {
    static int32_t bottom = -1;
    static int32_t top = -1;
    DMA1->IFCR = DMA_IFCR_CGIF1;
    int phase_correction = 0;
    GPIOB->BSRR = (1<<7);
    uint16_t *data = DMA1->ISR & DMA_ISR_HTIF1 ? &adc_data[0] : &adc_data[192];

    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;

        int num_edges = 0;
        int edge_indices[24];

        int state = 0;
        ssize_t run_start = -1;
        int last_v = -1;
        for (ssize_t i=0; i<192-1; i++) {
            uint32_t a = data[i], b = 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 = 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) {
                    uint32_t v0 = 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;
                }
            }
        }

        if (num_edges > 1) {
            const int approx_cycle = 32*16*125/2;
            int delta_avg = 0;
            for (size_t i=0; i<num_edges-1; i++) {
                int delta = edge_indices[i+1] - edge_indices[i];
                if (delta > approx_cycle/2 * 50) {
                    asm volatile ("bkpt");
                }
                while (delta > approx_cycle/2) {
                    delta -= approx_cycle;
                }
                delta_avg += delta;
            }
            delta_avg /= num_edges-1;

            int cycle = approx_cycle + delta_avg;
            int offset = 0;
            int phase_avg = 0;
            for (size_t i=0; i<num_edges; i++) {
                int remainder = edge_indices[i] - offset;
                while (remainder > cycle/2) {
                    remainder -= cycle;
                    offset += cycle;
                }
                phase_avg += remainder;
            }
            phase_avg /= num_edges;
            phase_correction = phase_avg >= 0 ? cycle/2 - phase_avg : cycle/2 + phase_avg;
            phase_correction /= 125 * 4;

            const int deadzone = 3;
            const int max_correction = 40;
            if (phase_correction < -deadzone || phase_correction > deadzone) {
                if (phase_correction < -max_correction) {
                    phase_correction = -max_correction;
                } else if (phase_correction > max_correction) {
                    phase_correction = max_correction;
                }
            }
        }
    }

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

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

    bottom /= keep;
    top /= keep;

    TIM3->ARR = 125*32 + phase_correction; 
    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;
        }
    }
}