#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 "LCD2x16.c"
#include "math.h"

#define Blen 127
short int XR[Blen+1], XP[Blen+1], XRAC[Blen+1], XPAC[Blen+1], k = 0;
float w[32];
float R0[Blen+1], R90[Blen+1], M0[Blen+1], M90[Blen+1], FM0[Blen+1], FM90[Blen+1];
float FM0ave, FM90ave, FM0acc, FM90acc;
short PhaseDisplay = 0;

// declare and init IIR weights: 4th order, Chebishew, Low Pass, reference [1]
// +/- 0.5%, -3dB at 0.025 (250Hz) of sampling frequency (10kHz)
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 ()  {                  
//int del, sw, Fp, j;

  // calculate Hilbert core for FIR filter
  w[0] = 0;                 // central weight: 2 x fc / fs
  for (short m = 1; m < 32; m++)        
    w[m] = (-1.0 + cos(pi * m)) / (pi * m);  // other weights
  for (short m = 1; m < 32; m++)        
    w[m] = w[m] * cos(pi / 2 * m / 31.0);       // windowing

  LCD_init (); 
  LCD_string("Phase=", 0x40); 

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

  while (1) {                            // endless loop
    if (PhaseDisplay) {
      PhaseDisplay = 0;
      float Phase = (atan (FM90ave / FM0ave)) / 3.14159 * 180;
      if (Phase < 0)    {LCD_string("-", 0x47);    Phase = -Phase; }
      else              {LCD_string(" ", 0x47);                    };
      short PhaseInt = (int)(Phase);
      short PhaseFrc = (int)((Phase - PhaseInt) * 10);
      LCD_sInt3DG((int)PhaseInt, 0x48, 1); 
      LCD_char('.');    LCD_char(PhaseFrc + '0'); LCD_char(0xdf);
      for (int j = 0; j<2000000; j++){};      // waste some time
    };
  };
}

// IRQ function for ADC
void ADC_IRQHandler(void)       
{
  GPIOE->BSRRL = BIT_8;                  // only about 6 us !

  XR[k] = ADC1->DR;                      // to clear ADC IRQ flag
  XP[k] = (ADC2->DR & 0xfff);            // acquire
  XRAC[k] = XR[k] - XR[(k-1) & Blen];    // remove DC component
  XPAC[k] = XP[k] - XP[(k-1) & Blen];    // remove DC component
  
  // derive cos component using Hilbert transform
  float conv = (float)XRAC[(k - 32) & Blen] * w[0];
  for (short m = 1; m < 32; m += 2)
    conv += w[m] *(XRAC[(k - 32 + m) & Blen] - XRAC[(k - 32 - m) & Blen]);
  R0[k] = (float)XRAC[(k - 32) & Blen];
  R90[k] = conv;
  
  M0[k]  = (float)XPAC[(k - 32) & Blen] * R0[k];    // multiply with sin
  M90[k] = (float)XPAC[(k - 32) & Blen] * R90[k];   // multiply with cos
  
  // IIR low-pass filter for sine component
  conv = 0;
  for (short m = 0; m<5; m++)   conv += a[m] * M0[(k - m) & Blen];
  for (short n = 1; n<5; n++)   conv += b[n] * FM0[(k - n) & Blen];
  FM0[k] = conv;
  FM0acc += conv;
  
  // IIR low-pass filter for cosine component
  conv = 0;
  for (short m = 0; m<5; m++)   conv += a[m] * M90[(k - m) & Blen];
  for (short n = 1; n<5; n++)   conv += b[n] * FM90[(k - n) & Blen];
  FM90[k] = conv;
  FM90acc += conv;
  
  // average and report result every 128-th time
  if (k == 0)   {
    FM0ave  = FM0acc;  FM0acc  = 0;
    FM90ave = FM90acc; FM90acc = 0;
    PhaseDisplay = 1;
  };
  
  k++; k &= Blen;
  GPIOE->BSRRH = BIT_8;
}