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

int Table[4096], TablePtr, K = 655, dK = 0;
char InPat = 0;
int PortE, f_VCO;

int main ()  {
  
  // Table init
  for (TablePtr = 0; TablePtr <= 4095; TablePtr++)
    Table[TablePtr] = (int)(1850.0 * sin((float)TablePtr / 2048.0 * 3.141592653));
  
  // GPIO clock enable, digital pin definitions
  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
  GPIOE->MODER |= 0x10000000;   // output pin PE14: f_out
  GPIOA->MODER |= 0x00001000;   // output pin PA06: LED D390
  
  // LCD init
  LCD_init(); LCD_string("F in=", 0x01);
  
  // DAC set-up
  RCC->APB1ENR |= 0x20000000;   // Enable clock for DAC
  DAC->CR      |= 0x00010001;   // DAC control reg, both channels ON
  GPIOA->MODER |= 0x00000f00;   // PA04, PA05 are analog outputs

  // Timer 2 set-up
  RCC->APB1ENR |= 0x0001;       // Enable clock for Timer 2
  TIM2->ARR     = 840;          // Auto Reload: 8400 == 100us -> 100kHz
  TIM2->DIER   |= 0x0001;       // DMA/IRQ Enable Register - enable IRQ on update
  TIM2->CR1    |= 0x0001;       // Enable Counting

  // NVIC IRQ enable
  NVIC_EnableIRQ(TIM2_IRQn);    // Enable IRQ for TIM2 in NVIC 

  // endless loop - display parameters
  while (1) {
    if (GPIOE->IDR & 0x0001) GPIOA->ODR |=  0x0040;   // LED on
    else                     GPIOA->ODR &= ~0x0040;   // else LED off
    LCD_uInt16((int)(K * 100000 / 65536),0x08,1);     // display frequency
    for (int i = 0; i < 500000; i++) {};              // waste time
  };
}

// IRQ function
void TIM2_IRQHandler(void)      // PLL takes approx 800 ns of CPU time!
{
  GPIOE->ODR |=  0x0100;        // PE08 up
  TIM2->SR &= ~0x00000001;      // clear update event flag in TIM2
  PortE = GPIOE->IDR & 0x8000;  // read input signal
  f_VCO = TablePtr   & 0x8000;  // this is locally generated signal
  InPat = (InPat << 2) & 0x0c;  // construct pattern for phase detector
  if (PortE) InPat += 2;        //
  if (f_VCO) InPat += 1;        //
  if (PortE == f_VCO)       dK =  0;    // if equal signals (frequencies)
  else { if (InPat == 0x02) dK =  1;    // if frequency too high
         if (InPat == 0x0d) dK =  1;    // 
         if (InPat == 0x01) dK = -1;    // if frequency too low
         if (InPat == 0x0e) dK = -1;    //
  };
  K += dK;                              // correct time interval
  if (K > 0x8000) K = 0x8000;           // but not too much
  if (K < 0x0080) K = 0x0080;           //
  TablePtr = (TablePtr + K + (dK << 8)) & 0xffff;    // update pointer to table
  GPIOE->ODR = (GPIOE->ODR & ~0x4000) | (f_VCO >> 1);// digital out - f_VCO
  DAC->DHR12R1 = (Table[TablePtr >> 4]) + 2048;      // analog signal -> DAC
  DAC->DHR12R2 = K >> 2;                             // frequency -> DAC
  GPIOE->ODR &= ~0x0100;        // PE08 down
}