#include "board.h"

/* private defines */
/* private variables */
static volatile uint32_t TDelay;

/* private typedef */
/* private functions */
static void GPIO_Init(void);
static void ADC_Init(void);
static void TIM1_Init(void);
static void TIM3_Init(void);
static void IWDG_Init(void);

/* Board perephireal Configuration  */
void Board_Init(void)
{
  /* Main peripheral clock enable */
  RCC->APBENR1 = (RCC_APBENR1_PWREN | RCC_APBENR1_TIM3EN);
  RCC->APBENR2 = (RCC_APBENR2_SYSCFGEN | RCC_APBENR2_ADCEN | RCC_APBENR2_TIM1EN);
  /* GPIO Ports Clock Enable */
  RCC->IOPENR = (RCC_IOPENR_GPIOAEN | RCC_IOPENR_GPIOBEN);

  /* Peripheral interrupt init*/
  /* RCC_IRQn interrupt configuration */
  NVIC_SetPriority(RCC_IRQn, 0);
  NVIC_EnableIRQ(RCC_IRQn);

  /* Configure the system clock */
  SystemClock_Config();

  /* Configure SysTick */
  SysTick->LOAD  = (uint32_t)(SystemCoreClock/1000 - 1UL);          /* set reload register */
  NVIC_SetPriority (SysTick_IRQn, (1UL << __NVIC_PRIO_BITS) - 1UL); /* set Priority for Systick Interrupt */
  SysTick->VAL   = 0UL;                                             /* Load the SysTick Counter Value */
  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
                   SysTick_CTRL_TICKINT_Msk   |
                   SysTick_CTRL_ENABLE_Msk;                         /* Enable SysTick IRQ and SysTick Timer */

  /* Processor uses sleep as its low power mode */
  SCB->SCR &= ~((uint32_t)SCB_SCR_SLEEPDEEP_Msk);
  /* DisableSleepOnExit */
  SCB->SCR &= ~((uint32_t)SCB_SCR_SLEEPONEXIT_Msk);

  /* Initialize all configured peripherals */
  GPIO_Init();

  ADC_Init();

  TIM1_Init();
  TIM3_Init();

  IWDG_Init();
}


/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  /* HSI configuration and activation */
  RCC->CR |= RCC_CR_HSION; // Enable HSI
  while((RCC->CR & RCC_CR_HSIRDY) == 0);

  /* Main PLL configuration and activation */
  RCC->PLLCFGR = (RCC_PLLCFGR_PLLSRC_HSI | RCC_PLLCFGR_PLLM_0 | (9 << RCC_PLLCFGR_PLLN_Pos) | RCC_PLLCFGR_PLLR_1);
  RCC->PLLCFGR |= RCC_PLLCFGR_PLLREN; // RCC_PLL_EnableDomain_SYS
  RCC->CR |= RCC_CR_PLLON; // RCC_PLL_Enable
  while((RCC->CR & RCC_CR_PLLRDY) == 0);

  /* Sysclk activation on the main PLL */
  RCC->CFGR &= RCC_CFGR_SW;
  RCC->CFGR |= RCC_CFGR_SW_1;
  while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_1);

  /* Update CMSIS variable (which can be updated also through SystemCoreClockUpdate function) */
  SystemCoreClock = 24000000;
}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void GPIO_Init(void)
{
  /* Servo_1_Pin, Servo_2_Pin - Servos control, Alt PP out, middle speed */
  GPIO_SetPinMode(Servo_1_Port, (Servo_1_Pin|Servo_2_Pin), GPIO_MODE_AFF);
  GPIO_SetPinOutputType(Servo_1_Port, (Servo_1_Pin|Servo_2_Pin), GPIO_OTYPE_PP);
  GPIO_SetPinSpeed(Servo_1_Port, (Servo_1_Pin|Servo_2_Pin), GPIO_OSPEED_LW);
  GPIO_SetPinPull(Servo_1_Port, (Servo_1_Pin|Servo_2_Pin), GPIO_PUPDR_NO);
  GPIO_SetAFPin_0_7(Servo_1_Port, (Servo_1_Pin|Servo_2_Pin), GPIO_AF_2);

  /* Photo_Pin: analog in, pull none */
  GPIO_SetPinPull(Photo_Port, Photo_Pin, GPIO_PUPDR_NO);
  GPIO_SetPinMode(Photo_Port, Photo_Pin, GPIO_MODE_ANL);

  /* Test out Pin A4 */
  GPIO_SetPinMode(GPIOA, GPIO_PIN_4, GPIO_MODE_OUT);
  GPIO_SetPinOutputType(GPIOA, GPIO_PIN_4, GPIO_OTYPE_PP);
  GPIO_SetPinSpeed(GPIOA, GPIO_PIN_4, GPIO_OSPEED_LW);
  GPIO_SetPinPull(GPIOA, GPIO_PIN_4, GPIO_PUPDR_NO);

  /* Test in Pin A5 */
  GPIO_SetPinPull(GPIOA, GPIO_PIN_4, GPIO_PUPDR_UP);
}

/**
  * @brief ADC1 Initialization Function
  * @param None
  * @retval None
  */
static void ADC_Init(void)
{
  /* ADC1 interrupt Init */
  NVIC_SetPriority(ADC1_IRQn, 0);
  NVIC_EnableIRQ(ADC1_IRQn);

  /* Configure the global features of the ADC
   * (Clock, Resolution, Data Alignment and number of conversion)
   */
  //ADC synchronous clock derived from AHB clock divided by 2
  ADC1->CFGR2 |= ADC_CFGR2_CKMODE_0;

  /* Poll for ADC channel configuration ready */
  while ((ADC1->ISR & ADC_ISR_CCRDY) == 0) {};
  /* Clear flag ADC channel configuration ready */
  ADC1->ISR |= ADC_ISR_CCRDY;

  ADC1->CFGR1 |= (ADC_CFGR1_EXTSEL_1 | ADC_CFGR1_EXTSEL_0 | ADC_CFGR1_EXTEN_0) | ADC_CFGR1_OVRMOD | ADC_CFGR1_EXTEN_0;

   /* Enable ADC internal voltage regulator */
   ADC1->CR |= ADC_CR_ADVREGEN; // ???

  /** Configure Regular Channel */
  ADC1->CHSELR = ADC_CHSELR_CHSEL11;

   /* Poll for ADC channel configuration ready */
  while ((ADC1->ISR & ADC_ISR_CCRDY) == 0) {};
  /* Clear flag ADC channel configuration ready */
  ADC1->ISR |= ADC_ISR_CCRDY;

  // SetChannelSamplingTime
  ADC1->SMPR |= ADC_SMPR_SMPSEL11;

  /* Calibration in single conversion mode */
  ADC1->CR |= ADC_CR_ADCAL; // ADC calibration
  while ((ADC1->CR & ADC_CR_ADCAL) != 0) {};

  /* Enable Interrupt */
  ADC1->IER |= ADC_IER_EOCIE; // ADC interrupt enable
  /* Enable ADC */
  ADC1->CR |= ADC_CR_ADEN; // ADC enable
  /* Start ADC software conversion */
  ADC1->CR |= ADC_CR_ADSTART; // ADC start
}

/**
  * @brief TIM1 Initialization Function
  * @param None
  * @retval None
  */
static void TIM1_Init(void)
{
  /* target clock */
  TIM1->PSC = TIM1_PSC; // prescaler
  TIM1->ARR = TIM1_ARR; // auto reload value
  TIM1->CR1 = TIM_CR1_ARPE;
  // initial pwm value
  TIM1->CCR1 = SERVO_INIT_VAL;
  TIM1->CCR4 = SERVO_INIT_VAL;
  // pwm mode 1 for chanels
  TIM1->CCMR1 = (TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1PE);
  TIM1->CCMR2 = (TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4PE);
  //TIM1->SR |= TIM_SR_UIF;
  TIM1->BDTR = TIM_BDTR_MOE; // enable main output
  TIM1->EGR = TIM_EGR_UG; // force timer update
  /* TIM1 CC_EnableChannel */
  TIM1->CCER = (TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC4E | TIM_CCER_CC4P);
  /* TIM_EnableCounter */
  TIM1->CR1 |= TIM_CR1_CEN;
}

/**
  * @brief TIM3 Initialization Function
  * @param None
  * @retval None
  */
static void TIM3_Init(void)
{
  /* target clock */
  TIM3->PSC = TIM3_PSC; // prescaler
  TIM3->ARR = TIM3_ARR; // auto reload value
  TIM3->CR1 = TIM_CR1_ARPE;
  // launch timer
  TIM3->EGR = TIM_EGR_UG; // force timer update
  /* Set the trigger output 2 (TRGO2) used for ADC synchronization */
  TIM3->CR2 |= TIM_CR2_MMS2_2; // update event
  /* TIM3 enable */
  TIM3->CR1 |= TIM_CR1_CEN;
}

/**
  * @brief IWDG Initialization Function
  * @param None
  * @retval None
  */
static void IWDG_Init(void) 
{
  IWDG->KR = 0xCCCC;
  IWDG->KR = 0x5555;
  IWDG->PR = 0x0;
  IWDG->RLR = 4095;
  while (IWDG->SR != 0x00000000) {};

  IWDG->KR = 0xAAAA;
}

/**
  * @brief  Inserts a delay time.
  * @param  msec: specifies the delay time length, in milliseconds.
  * @retval None
  */
void delay_ms(uint32_t msek) {
  TDelay = msek;

  do {
    __WFI();
  } while (TDelay != 0);
}

#pragma GCC optimize ("O3")
/**
  * @brief  This function handles SysTick Handler.
  * @param  None
  * @retval None
  */
void SysTick_Handler(void) {
  if (TDelay != 0) {
    TDelay --;
  }
}