#include "stm32f4xx.h"
#include "math.h"
#include "LCD2x16.c"

#define pi 3.141592653589
#define B00 0x0001
#define B01 0x0002
#define B02 0x0004
#define B03 0x0008
#define B04 0x0010
#define B05 0x0020

int   Ref[64], x[64], Ptr;  // 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;

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 set-up
  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 

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

  // set gains & waste time - indefinite loop
  while (1) {
  if ((GPIOE->IDR & 0x003f) == (B00 + B05)) Kp++;         // manually set Kp
  if ((GPIOE->IDR & 0x003f) == (B00 + B04)) Kp--;
  if (Kp<0) Kp = 0;     if (Kp > 1000) Kp = 1000;
  if ((GPIOE->IDR & 0x003f) == (B01 + B05)) Kd += 0.001;  // manually set Kd
  if ((GPIOE->IDR & 0x003f) == (B01 + B04)) Kd -= 0.001;
  if (Kd < 0) Kd = 0;   if (Kd > 1) Kd = 1;
  if ((GPIOE->IDR & 0x003f) == (B02 + B05)) Ki += 0.0001; // manually set Ki
  if ((GPIOE->IDR & 0x003f) == (B02 + B04)) 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 < 2000000; i++)      {};              // waste time
  };
}

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