#include "stm32f4xx.h"
#include "stm32f4xx_rcc.c"
#include "stm32f4xx_adc.c"
#include "stm32f4xx_dac.c"
#include "stm32f4xx_gpio.c"
#include "stm32f4xx_tim.c"
#include "dd.h"
#include "math.h"

short int X1[64], X2[64], k = 0;
float Y0[64], Y90[64], FY0[64], FY90[64]; //, Div[64];
int Table[4096], ptrTable;   
//int Result, kk = 12000;

// declare and init IIR weights: 4th order, Chebishew, Low Pass, reference [1]
// +/- 0.5%, -3dB at 0.025 (2500Hz) of sampling frequency
float a[5] = {1.504626e-5, 6.018503e-5, 9.027754e-5, 6.018503e-5, 1.504626e-5};
float b[5] = {0          , 3.725385e0, -5.226004e0 , 3.270902e0, -7.705239e-1};

void SWITCHinit (void)  {
GPIO_InitTypeDef        GPIO_InitStructure;
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_3 | GPIO_Pin_4 | 
                                  GPIO_Pin_5 | GPIO_Pin_6;
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_IN;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_DOWN;
  GPIO_Init(GPIOE, &GPIO_InitStructure);  
}

// ADC init function
void ADCinit_T5_CC1_IRQ (void)      {
ADC_InitTypeDef         ADC_InitStructure;
GPIO_InitTypeDef        GPIO_InitStructure;
ADC_CommonInitTypeDef   ADC_CommonInitStructure;
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1  | RCC_APB2Periph_ADC2,  ENABLE);
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB, ENABLE);
  
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_1 | GPIO_Pin_2;
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AN;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_Init(GPIOA, &GPIO_InitStructure); 
  
  ADC_CommonInitStructure.ADC_DMAAccessMode    = ADC_DMAAccessMode_Disabled;
  ADC_CommonInitStructure.ADC_Mode             = ADC_DualMode_RegSimult;
  ADC_CommonInitStructure.ADC_Prescaler        = ADC_Prescaler_Div2;
  ADC_CommonInit(&ADC_CommonInitStructure);
    
  ADC_InitStructure.ADC_Resolution             = ADC_Resolution_12b;
  ADC_InitStructure.ADC_ScanConvMode           = DISABLE;
  ADC_InitStructure.ADC_ContinuousConvMode     = DISABLE;
  ADC_InitStructure.ADC_ExternalTrigConvEdge   = ADC_ExternalTrigConvEdge_Rising;
  ADC_InitStructure.ADC_ExternalTrigConv       = ADC_ExternalTrigConv_T5_CC1;
  ADC_InitStructure.ADC_DataAlign              = ADC_DataAlign_Right;
  ADC_InitStructure.ADC_NbrOfConversion        = 1;
  ADC_Init(ADC1, &ADC_InitStructure);
  ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 1, ADC_SampleTime_3Cycles);

  ADC_Init(ADC2, &ADC_InitStructure);
  ADC_RegularChannelConfig(ADC2, ADC_Channel_2, 1, ADC_SampleTime_3Cycles);

  ADC_Cmd(ADC1, ENABLE);
  ADC_Cmd(ADC2, ENABLE);

  NVIC_EnableIRQ(ADC_IRQn);                // Enable IRQ for ADC in NVIC 
  ADC_ITConfig(ADC1, ADC_IT_EOC, ENABLE);  // Enable IRQ generation in ADC 
}

// DAC init function
void DACinit (void)     {
DAC_InitTypeDef         DAC_InitStructure;
GPIO_InitTypeDef        GPIO_InitStructure;
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_4 | GPIO_Pin_5;
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_AN;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;  
  GPIO_Init(GPIOA, &GPIO_InitStructure);  
  
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
  DAC_InitStructure.DAC_OutputBuffer                 = DAC_OutputBuffer_Enable;
  DAC_InitStructure.DAC_Trigger                      = DAC_Trigger_None;
  DAC_InitStructure.DAC_WaveGeneration               = DAC_WaveGeneration_None;
  DAC_Init(DAC_Channel_1, &DAC_InitStructure); 
  DAC_Init(DAC_Channel_2, &DAC_InitStructure);
  
  DAC_Cmd(DAC_Channel_1, ENABLE);   
  DAC_Cmd(DAC_Channel_2, ENABLE);
}

// Timer 5 init function - time base
void TIM5init_TimeBase_CC1 (int interval)  {
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
TIM_OCInitTypeDef       TIM_OCInitStructure;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5,  ENABLE);
  TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
  TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInitStructure.TIM_Period = interval; 
  TIM_TimeBaseInitStructure.TIM_Prescaler = 0;
  TIM_TimeBaseInitStructure.TIM_RepetitionCounter = 0;
  TIM_TimeBaseInit(TIM5, &TIM_TimeBaseInitStructure);

  TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
  TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
  TIM_OCInitStructure.TIM_OutputNState = TIM_OutputState_Disable;
  TIM_OCInitStructure.TIM_Pulse = 1;
  TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
  TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_Low;
  TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Reset;
  TIM_OCInitStructure.TIM_OCNIdleState =  TIM_OCIdleState_Set;
  TIM_OC1Init(TIM5, &TIM_OCInitStructure);

  TIM_Cmd(TIM5, ENABLE);
}   

// initialize port E, Gpio_Pin_8 to GPIO_Pin_15 as outputs
void GPIOEinit (void)   {
GPIO_InitTypeDef  GPIO_InitStructure;
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_8  | GPIO_Pin_9  | GPIO_Pin_10 | GPIO_Pin_11;
  GPIO_InitStructure.GPIO_Pin  |= GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_Init(GPIOE, &GPIO_InitStructure); 
}

#define pi 3.14159
int main ()  {                  
  for (ptrTable = 0; ptrTable <= 4095; ptrTable++)
    Table[ptrTable] = (int)(1850.0 * sin((float)ptrTable / 2048.0 * 3.14159265));

  GPIOEinit ();
  //SWITCHinit ();
  ADCinit_T5_CC1_IRQ();
  DACinit();
  TIM5init_TimeBase_CC1(840);           // 840 == 10us == 100kHz

  while (1) {                            // endless loop
  };
}

// IRQ function for ADC
void ADC_IRQHandler(void)       
{
  GPIOE->BSRRL = BIT_8;         // about 8 us
  int Spare = ADC1->DR;                         // to clear ADC IRQ flag

  X1[k] = (ADC2->DR & 0xfff);                   // acquire
  X2[k] = X1[k] - X1[(k-1) & 63];               // remove DC component
  
  Y0[k]  = X2[k] * Table[  ptrTable >> 4                ];  // multiply with sin
  Y90[k] = X2[k] * Table[((ptrTable >> 4) + 1024) & 4095];  // multiply with cos
  
  float conv = 0;                               // LP filter (sin)
  for (int m = 0; m<5; m++)  conv += a[m] * Y0[(k - m) & 63];
  for (int n = 1; n<5; n++)  conv += b[n] * FY0[(k - n) & 63];
  FY0[k] = conv;

  conv = 0;                                     // LP filter (cos)
  for (int m = 0; m<5; m++)  conv += a[m] * Y90[(k - m) & 63];
  for (int n = 1; n<5; n++)  conv += b[n] * FY90[(k - n) & 63];
  FY90[k] = conv;
  
  float Res = FY0[k] * FY0[k] + FY90[k] * FY90[k];      // add geometrically
  float ResRes = (float)(sqrt((double)Res));    // sqrt takes about 4 us !

  DAC->DHR12R1 = Table[ptrTable >> 4] + 2048;          // local oscillator
  DAC->DHR12R2 = (int)(ResRes / 64 + 128.0);    // demodulated out

  ptrTable = (ptrTable + 12000) & 0xffff;          // next excitation

  k++; k &= 63;
  GPIOE->BSRRH = BIT_8;
}