#include "stm32f4xx.h"
#include "math.h"
#define pi 3.141592653589

int   x1[4096], x2[4096], xyPtr;  // declare circular buffers
float y1[4096], y2[4096];         // declare circular buffers
// declare and init IIR weights: 4th order, Chebishew, Low Pass
// 0.5%, -3 dB at 0.025 (250 Hz here) of sampling frequency
const float w1x[5] = {1.504626e-5,  6.018503e-5,  9.027754e-5,  6.018503e-5,  1.504626e-5};          
const float w1y[5] = {0          ,  3.725385e0 , -5.226004e0 ,  3.270902e0 , -7.705239e-1};              

void ADC_setup(void)    {
  RCC->APB2ENR |= 0x00000100;   // clock for ADC1
  RCC->APB2ENR |= 0x00000200;   // clock for ADC2
  ADC->CCR      = 0x00000006;   // Regular simultaneous mode only
  ADC1->CR2     = 0x00000001;   // ADC1 ON
  ADC1->SQR3    = 0x00000002;   // use PA02 as input
  ADC2->CR2     = 0x00000001;   // ADC1 ON
  ADC2->SQR3    = 0x00000003;   // use PA03 as input
  GPIOA->MODER |= 0x000000f0;   // PA02, PA03 are analog inputs

  ADC1->CR2    |= 0x06000000;   // use TIM2, TRG0 as SC source
  ADC1->CR2    |= 0x10000000;   // Enable external SC, rising edge
  ADC1->CR1    |= 0x00000020;   // Enable ADC Interrupt for EOC
}

void DAC_setup(void)    {
  RCC->APB1ENR |= 0x20000000;   // Enable clock for DAC
  DAC->CR      |= 0x00010001;   // DAC control reg, both channels ON
  GPIOA->MODER |= 0x00000f00;   // PA04, PA05 are analog outputs
}

void GPIO_setup(void)   {
  RCC->AHB1ENR |= 0x00000001;   // Enable clock for GPIOA
  RCC->AHB1ENR |= 0x00000010;   // Enable clock for GPIOE
  GPIOE->MODER |= 0x00010000;   // output pin PE08: time mark
  GPIOE->MODER |= 0x00040000;   // output pin PE09: toggle
  GPIOA->MODER |= 0x00001000;   // output pin PA06: LED D390
}

void Timer2_setup(void) {
  RCC->APB1ENR |= 0x0001;       // Enable clock for Timer 2
  TIM2->ARR     = 8400;         // Auto Reload value: 8400 == 100us
  TIM2->CR2    |= 0x0020;       // select TRGO to be update event (UE)
  TIM2->CR1    |= 0x0001;       // Enable Counting
}

int main ()  {
  GPIO_setup();         // GPIO clock enable, digital pin definitions
  DAC_setup();          // DAC set-up
  ADC_setup();          // ADC set-up
  Timer2_setup();       // Timer 2 set-up
  NVIC_EnableIRQ(ADC_IRQn);     // Enable IRQ for ADC in NVIC 

  // waste time - indefinite
  while (1) {
    if (GPIOE->IDR & 0x0001) GPIOA->ODR |=  0x0040;   // LED on
    else                     GPIOA->ODR &= ~0x0040;   // else LED off
  GPIOE->ODR   |=  0x0200;      // PE09 up
  GPIOE->ODR   &= ~0x0200;      // PE09 down
  };
}

// IRQ function
void ADC_IRQHandler(void)       // this takes approx 6us of CPU time!
{
  GPIOE->ODR |=  0x0100;        // PE08 up
  x1[xyPtr]  = ADC1->DR;        // pass ADC -> circular buffer x1
  x2[xyPtr]  = ADC2->DR;        // pass ADC -> circular buffer x2
  float conv = 0;                           // declare and init sum
  for (int i = 0; i < 5; i++)               // for koefs 0 to 4
    conv += w1x[i] * x1[(xyPtr-i) & 4095];  // convolve inputs
  for (int i = 1; i < 5; i++)               // for koefs 1 to 4
    conv += w1y[i] * y1[(xyPtr-i) & 4095];  // convolve outputs
  y1[xyPtr] = conv;                         // save filtered result
  y2[xyPtr] = (float)x1[xyPtr];             // save original
  DAC->DHR12R1  = (int)y1[xyPtr];           // filtered -> DAC
  DAC->DHR12R2  = (int)y2[xyPtr];           // original -> DAC
  xyPtr = (xyPtr + 1) & 4095;   // increment pointer to circulat buffer
  GPIOE->ODR &= ~0x0100;        // PE08 down
}