#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"

#define pi 3.141592653589

int   Ref[64], X[64], k;               // declare circular buffers
float Kp = 1.0, Ki = 0.0, Kd = 0.0;     // declare & init params
int Error[64];                          // declare error vector
int Reg[64];                            // declare past output vector
float Prop, Dif, Int = 0;               // declare (& init) vars
float Ts = 0.0001;                      // defined by constant 8400 in TIM2->arr;

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

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

int main ()  {
int sw;
  GPIOEinit ();
  SWITCHinit();
  ADCinit_T5_CC1_IRQ();
  DACinit();
  TIM5init_TimeBase_CC1(8400);           // 8400 == 100us == 10kHz

  LCD_init();
  LCD_string("Kp:", 0x00);
  LCD_string("Kd:", 0x09);
  LCD_string("Ki:", 0x49);

  // set gains & waste time - indefinite loop
  while (1) {
    sw = GPIOE->IDR;
    if ((sw & S370) && !(sw & S372) && !(sw & S373)) Kp--;   // manually set Kp
    if ((sw & S371) && !(sw & S372) && !(sw & S373)) Kp++;
    if (Kp<0) Kp = 0;     if (Kp > 1000) Kp = 1000;
    if ((sw & S370) &&  (sw & S372) && !(sw & S373)) Kd -= 0.001;  // manually set Kd
    if ((sw & S371) &&  (sw & S372) && !(sw & S373)) Kd += 0.001;
    if (Kd < 0) Kd = 0;   if (Kd > 1) Kd = 1;
    if ((sw & S370) && !(sw & S372) &&  (sw & S373)) Ki -= 0.0001; // manually set Ki
    if ((sw & S371) && !(sw & S372) &&  (sw & S373)) Ki += 0.0001;
    if (Ki < 0) Ki = 0;   if (Ki > 1) Ki = 1;
    LCD_sInt3DG((int)Kp,0x03,1);                            // write Kp
    LCD_sInt3DG((int)(Kd*1000),0x0c,1);                     // write Kd
    LCD_sInt3DG((int)(Ki*10000),0x4c,1);                    // write Ki
    for (int i = 0; i < 5000000; i++)      {};              // waste time
  };
}

// IRQ function
void ADC_IRQHandler(void)               // this takes approx 6us of CPU time!
{
  GPIOE->ODR |=  0x0100;                // PE08 up
  Ref[k]  = ADC2->DR;                   // pass ADC -> circular buffer Ref
  X[k]  = ADC1->DR;                     // pass ADC -> circular buffer X
  // PID calculation start
  Error[k] = Ref[k] - X[k];             // calculate error
  Prop = Kp * (float)Error[k];          // proportional part
  Dif  = Kd * (float)(Error[k] - Error[(k-1) & 63]) / Ts;  // differential part
  Int += Ki * (float)Error[k];          // integral part
  Reg[k] = (int)(Prop + Dif + Int);     // summ all three
  // PID calculation stop
  if (Reg[k] > 4095) DAC->DHR12R1  = 4095;    // limit output due to the DAC
  else if (Reg[k] < 0) DAC->DHR12R1  = 0;
       else DAC->DHR12R1  = (int)Reg[k];      // regulator output -> DAC
  DAC->DHR12R2  = Error[k] + 2048;            // Error -> DAC
  k = (k + 1) & 63;                     // increment pointer to circular buffer
  GPIOE->ODR &= ~0x0100;                // PE08 down
}