#include "stm32f10x.h"
#include "math_int.h"
#include "lsm6ds3_spi.h"
#include "utils.h"

enum
{
  FALSE = 0,
  TRUE
};

enum
{
  REVERSE = 0,
  FORWARD
};

union
{
  uint32_t data[2];
  uint16_t value[4];
} adc;

#ifdef DEBUG
struct
{
  int16_t a, b, c, d;
} tx_data_a[100], \
  tx_data_b[100], \
  *p_tx_data = tx_data_a;

struct
{
  uint16_t a, b, c, d, e, f;
} rx_data;
#endif // DEBUG

int32_t rotor_position_sh;
int16_t rotor_step_sh, advance_sh;
uint16_t amplitude;

const uint16_t rotor_forward_position_lut[] = {0, 60, 301, 3, 176, 121, 241, 0}; // true rotor position aligned to D axis, forward motion
//const uint16_t rotor_reverse_position_lut[] = {0, 125, 10, 64, 243, 180, 303, 0}; // backward motion

const uint8_t hall_forward_future_lut[] = {0, 5, 3, 1, 6, 4, 2, 0}; // future hall state to find out direction of rotation
const uint8_t hall_reverse_future_lut[] = {0, 3, 6, 2, 5, 1, 4, 0};

volatile uint16_t tick = 0;

void SysTick_Handler(void)
{
  tick++;
}

void clock_init(void)
{
  FLASH->ACR |= FLASH_ACR_PRFTBE | FLASH_ACR_LATENCY_0; // FLASH prefetch buffer enable, one wait state

  RCC->CR |= RCC_CR_HSEON; // enable HSE oscillator
  while (!(RCC->CR & RCC_CR_HSERDY)) // wait for HSE oscillator to be ready
    ;

  RCC->CFGR |= RCC_CFGR_PLLMULL4 | RCC_CFGR_PLLSRC; // PLL x4, HSE input clock
  RCC->CR |= RCC_CR_PLLON; // enable PLL
  while (!(RCC->CR & RCC_CR_PLLRDY)) // wait for PLL to be ready
    ;
  RCC->CFGR |= RCC_CFGR_ADCPRE_DIV4 | RCC_CFGR_SW_1; // ADC clock PCLK1 / 4 = 8 MHz, switch to PLL, SYSCLK = HCLK = PCLK1 = PCLK2 = PLL clock 32 MHz

  RCC->AHBENR |= RCC_AHBENR_DMA1EN; // enable DMA1 clock
  // enable SPI1, TIM1, ADC2, ADC1, GPIOC, GPIOB, GPIOA and AFIO clock
  RCC->APB2ENR |= RCC_APB2ENR_SPI1EN | RCC_APB2ENR_TIM1EN | RCC_APB2ENR_ADC2EN | RCC_APB2ENR_ADC1EN | RCC_APB2ENR_IOPCEN | RCC_APB2ENR_IOPBEN | RCC_APB2ENR_IOPAEN | RCC_APB2ENR_AFIOEN;
  RCC->APB1ENR |= RCC_APB1ENR_USART3EN | RCC_APB1ENR_TIM4EN | RCC_APB1ENR_TIM2EN; // enable USART3, TIM4 and TIM2 clock
}

void gpio_init(void)
{
  AFIO->MAPR = AFIO_MAPR_SWJ_CFG_JTAGDISABLE; // SW-DP enabled, JTAG disabled
  GPIOA->BSRR = GPIO_BSRR_BR15; // reset pull-up on PA15 (JTDI)
  GPIOA->CRH &= ~GPIO_CRH_CNF15; // clear all used
  GPIOA->CRH |= GPIO_CRH_MODE15_0; // PA15 (BUZZ, OUTPUT-PP), medium speed (10 MHz)

  GPIOC->CRH &= ~(GPIO_CRH_CNF15 | GPIO_CRH_CNF14); // clear all used
  GPIOC->CRH |= GPIO_CRH_MODE15_0 | GPIO_CRH_MODE14_0; // PC15 (LED_B, OUTPUT-PP), PC14 (LED_A, OUTPUT-PP), medium speed (10 MHz)
}

void adc_init(void)
{
  GPIOA->CRL &= ~(GPIO_CRL_CNF3 | GPIO_CRL_CNF2 | GPIO_CRL_CNF1 | GPIO_CRL_CNF0); // clear all used

  ADC1->SMPR2 |= ADC_SMPR2_SMP2_0 | ADC_SMPR2_SMP0_0; // channel 2 and 0 sample time 7.5 cycles
  ADC2->SMPR2 |= ADC_SMPR2_SMP3_0 | ADC_SMPR2_SMP1_0; // channel 3 and 1 sample time 7.5 cycles

  ADC1->CR2 |= ADC_CR2_EXTTRIG | ADC_CR2_EXTSEL | ADC_CR2_DMA | ADC_CR2_ADON; // external software trigger, DMA and ADC1 enable
  ADC2->CR2 |= ADC_CR2_EXTTRIG | ADC_CR2_EXTSEL | ADC_CR2_ADON; // external software trigger, enable ADC2

  delay_ms(1); // delay before calibration

  ADC1->CR2 |= ADC_CR2_CAL; // start ADC1 calibration
  while(ADC1->CR2 & ADC_CR2_CAL) // wait until finished
    ;

  ADC2->CR2 |= ADC_CR2_CAL; // start ADC2 calibration
  while(ADC1->CR2 & ADC_CR2_CAL) // wait until finished
    ;
}

void adc_read(void)
{
  uint8_t i;

  for(i = 0; i < 4; i += 2)
  {
    ADC1->CR1 &= ~ADC_CR1_DUALMOD; // disable ADC1 & ADC2 combination to prevent loss of synchronization
    ADC1->SQR3 = i; // set proper channel for one conversion
    ADC2->SQR3 = i + 1;
    ADC1->CR1 |= ADC_CR1_DUALMOD_2 | ADC_CR1_DUALMOD_1; // enable ADC1 & ADC2 combination on regular channel(s)

    ADC1->CR2 |= ADC_CR2_SWSTART; // start ADC measurement
    while(!(ADC1->SR & ADC_SR_EOC)) // wait until finished
      ;
    adc.data[i >> 1] = ADC1->DR; // flag is cleared by reading the register
  }
}

#ifdef DEBUG
void uart_init(void)
{
  GPIOB->CRH &= ~(GPIO_CRH_CNF11 | GPIO_CRH_CNF10); // clear all used
  // PB11 (USART3 RX, INPUT), PB10 (USART3 TX, AF-PP), medium speed (10 MHz)
  GPIOB->CRH |= GPIO_CRH_CNF11_0 | GPIO_CRH_CNF10_1 | GPIO_CRH_MODE10_0;

  USART3->BRR |= 0x0116; // 8N1, 115200 baud @ 32 MHz
  USART3->CR3 |= USART_CR3_DMAT | USART_CR3_DMAR; // enable DMA
  USART3->CR1 |= USART_CR1_UE | USART_CR1_TE | USART_CR1_RE; // enable USART3 with transmitter and receiver

  DMA1_Channel2->CPAR = (uint32_t) &USART3->DR;
  DMA1_Channel2->CCR |= DMA_CCR2_MINC | DMA_CCR2_DIR; // low priority, increment and read from memory

  DMA1_Channel3->CPAR = (uint32_t) &USART3->DR;
  DMA1_Channel3->CCR |= DMA_CCR3_PL_0 | DMA_CCR3_MINC; // medium priority, increment memory and read from peripheral
}
#endif // DEBUG

void timer1_init(void)
{
  GPIOA->CRH &= ~(GPIO_CRH_CNF10 | GPIO_CRH_CNF9 | GPIO_CRH_CNF8); // clear all used
  // PA10 (TIM1 CH3, AF-PP), PA9 (TIM1 CH2, AF-PP), PA8 (TIM1 CH1, AF-PP), medium speed (10 MHz)
  GPIOA->CRH |= GPIO_CRH_CNF10_1 | GPIO_CRH_MODE10_0 | GPIO_CRH_CNF9_1 | GPIO_CRH_MODE9_0 | GPIO_CRH_CNF8_1 | GPIO_CRH_MODE8_0;

  GPIOB->CRH &= ~(GPIO_CRH_CNF15 | GPIO_CRH_CNF14 | GPIO_CRH_CNF13); // clear all used
  // PB15 (TIM1 CH3N, AF-PP), PB14 (TIM1 CH2N, AF-PP), PB13 (TIM1 CH1N, AF-PP), medium speed (10 MHz)
  GPIOB->CRH |= GPIO_CRH_CNF15_1 | GPIO_CRH_MODE15_0 | GPIO_CRH_CNF14_1 | GPIO_CRH_MODE14_0 | GPIO_CRH_CNF13_1 | GPIO_CRH_MODE13_0;

  TIM1->PSC = 1 - 1; // prescaler, 31.25 ns @ 32 MHz
  TIM1->ARR = 800; // 50 us -> 20 kHz (center-aligned mode -> T = 2 * 800 * 31.25 ns)
  TIM1->RCR = 1; // update only on overflow
  TIM1->CCR1 = 0;
  TIM1->CCR2 = 0;
  TIM1->CCR3 = 0;

  TIM1->DIER |= TIM_DIER_UIE; // update/overflow interrupt enable
  TIM1->CCMR1 |= TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2PE | TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1PE; // PWM mode 1, preload enable
  TIM1->CCMR2 |= TIM_CCMR2_OC3M_2 | TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3PE;
  TIM1->CCER |= TIM_CCER_CC3NE | TIM_CCER_CC3E | TIM_CCER_CC2NE | TIM_CCER_CC2E | TIM_CCER_CC1NE | TIM_CCER_CC1E; // enable outputs
  TIM1->BDTR |= TIM_BDTR_MOE | TIM_BDTR_DTG_5; // main output enable, dead-time 1 us
  TIM1->CR1 |= TIM_CR1_CMS_0 | TIM_CR1_CEN; // center-aligned mode, TIM1 enable
}

void TIM1_UP_IRQHandler(void)
{
  int16_t u, v, w;
  int32_t advanced_position_sh;

  TIM1->SR &= ~TIM_SR_UIF; // clear interrupt flag

  rotor_position_sh += rotor_step_sh;
  if(rotor_position_sh < 0)
    rotor_position_sh += 360 * 128;
  if(rotor_position_sh >= 360 * 128)
    rotor_position_sh -= 360 * 128;

  advanced_position_sh = rotor_position_sh + advance_sh; // can be negative
  if(advanced_position_sh < 0)
    advanced_position_sh += 360 * 128;
  if(advanced_position_sh >= 360 * 128)
    advanced_position_sh -= 360 * 128;

  bldc_sin_thi(advanced_position_sh / 128, &u, &v, &w);

  TIM1->CCR1 = 400 + (amplitude * u) / 1024;
  TIM1->CCR2 = 400 + (amplitude * v) / 1024;
  TIM1->CCR3 = 400 + (amplitude * w) / 1024;
}

void timer2_init(void)
{
  TIM2->PSC = 32 - 1; // prescaler, 1 us @ 32 MHz
  TIM2->ARR = 1000 - 1; // 1 ms -> 1 kHz

  TIM2->DIER |= TIM_DIER_UIE; // update/overflow interrupt enable
  TIM2->CR1 |= TIM_CR1_CEN; // TIM2 enable
}

void TIM2_IRQHandler(void)
{
  static int32_t imu_angle_sh = 0;
  int16_t imu_angle, torque, rotor_angle, I_u, I_v, I_w, temp, U_d, err_q, U_q, arg_sh;
  static uint8_t run = FALSE;
  static int32_t sum_angle = 0;
  static int16_t I_d = 0;
  static int16_t I_q = 0;
  static int32_t sum_d = 0;
  static int32_t sum_q = 0;
  uint16_t mod;
  static uint16_t batt_cnt = 0;
  uint8_t beeps;
  static uint16_t beep_cnt = 0;

#ifdef DEBUG
  static uint8_t i = 0;
  static uint16_t rx_timeout = 0;

  static uint16_t kp = 700;
  static uint16_t kd = 300;
  static uint16_t kpd = 65;
  static uint16_t kid = 60;
  static uint16_t kpq = 65;
  static uint16_t kiq = 60;
#else
  #define kp 700 // violates big letters convention
  #define kd 300
  #define kpd 65
  #define kid 60
  #define kpq 65
  #define kiq 60
#endif // DEBUG

  TIM2->SR &= ~TIM_SR_UIF; // clear interrupt flag

  // PID regulator with output based on rider's position
  lsm6ds3_read_data();
  
  imu_angle_sh -= (1792 * lsm6ds3_data.gyro.y) / 3125; // 0.0175 (dps) * 128 (decimal part) * 256 (8 shifts left) * 0.001 (dt)
  imu_angle_sh += (256 * atan2_int(-lsm6ds3_data.accel.x, -lsm6ds3_data.accel.z) - imu_angle_sh) / 512; // LPF, f(-3 dB) ~ 2 Hz @ fs = 1 kHz
  imu_angle = imu_angle_sh / 256;
  imu_angle = (int16_t) limit((int32_t) imu_angle, -30 * 128, 30 * 128); // limit +/- 30 degrees around 0 degrees
  
  if(run == TRUE)
  {
    sum_angle += expo(imu_angle);
    sum_angle = limit(sum_angle, -1500 * 2000, 1500 * 2000); // ~12 degrees for 2 seconds, also depends on final gain

    torque = (kp * imu_angle) / 128 + sum_angle / 2048 - (kd * lsm6ds3_data.gyro.y) / 1024; // PID
    torque = (int16_t) limit((int32_t) torque, -1603, 1603); // limit +/- 37 A
    
    if(GPIOB->IDR & GPIO_IDR_IDR12) // button released
    {
      run = FALSE;
      sum_angle = 0;
      torque = 0;
    }
  }
  else
  {
    torque = 0;
    if(!(GPIOB->IDR & GPIO_IDR_IDR12) && imu_angle > -64 && imu_angle < 64) // button pressed and angle is in range +/- 0.5 degree
      run = TRUE;
  }

  // read all necessary data
  NVIC_DisableIRQ(TIM4_IRQn); // disable TIM4 interrupt first, because it can re-enable TIM1 interrupt
  NVIC_DisableIRQ(TIM1_UP_IRQn); // disable TIM1 interrupt, both because of "read-while-write" protection

  rotor_angle = (rotor_position_sh - 18 * rotor_step_sh) / 128; // current rotor position with ACS712 RC filters delay compensation

  NVIC_EnableIRQ(TIM1_UP_IRQn); // enable interrupts again
  NVIC_EnableIRQ(TIM4_IRQn);

  if(rotor_angle < 0)
    rotor_angle += 360;
  if(rotor_angle >= 360)
    rotor_angle -= 360;

  // phase currents measurement
  adc_read();

  I_u = adc.value[0] - 2254; // subtract DC offsets
  I_v = adc.value[1] - 2254;
  I_w = adc.value[2] - 2240;

  // UVW to DQ transformation with filtering
  temp = (I_u * cos_int(0 - rotor_angle)) / 32768;
  temp += (I_v * cos_int(120 - rotor_angle)) / 32768;
  temp += (I_w * cos_int(240 - rotor_angle)) / 32768;
  I_d += (temp - I_d) / 8; // LPF, f(-3 dB) ~ 134 Hz @ fs = 1 kHz

  temp = (I_u * sin_int(0 - rotor_angle)) / 32768;
  temp += (I_v * sin_int(120 - rotor_angle)) / 32768;
  temp += (I_w * sin_int(240 - rotor_angle)) / 32768;
  I_q += (temp - I_q) / 8; // LPF, f(-3 dB) ~ 134 Hz @ fs = 1 kHz

  // D axis PI regulator (with inverted output!)
  sum_d += I_d; // should be divided by 1000 (~1024)
  sum_d = limit(sum_d, -20 * 1024, 20 * 1024); // also depends on final gain
  U_d = (kpd * I_d) / 128 + (kid * sum_d) / 1024; // PI
  U_d = (int16_t) limit((int32_t) U_d, -1024, 1024);

  // Q axis PI regulator
  err_q = torque - I_q; // current 42 A should match +1820 LSB
  sum_q += err_q; // should be divided by 1000 (~1024)
  sum_q = limit(sum_q, -20 * 1024, 20 * 1024); // also depends on final gain
  U_q = (kpq * err_q) / 128 + (kiq * sum_q) / 1024; // PI
  U_q = (int16_t) limit((int32_t) U_q, -1024, 1024);
  
  // modulus and argument calculation
  mod = sqrt_int(U_d * U_d + U_q * U_q);
  mod = (uint16_t) limit((int32_t) mod, 0, 1024);
  if(mod > 0)
  {
    arg_sh = atan2_int(U_q, U_d);
    
    if(U_q >= 0) // forward
    {
      arg_sh = 180 * 128 - arg_sh;
      arg_sh = (int16_t) limit((int32_t) arg_sh, 30 * 128, 150 * 128); // limit +/- 60 degrees around 90 degrees
    }
    else // reverse
    {
      arg_sh = -180 * 128 - arg_sh;
      arg_sh = (int16_t) limit((int32_t) arg_sh, -150 * 128, -30 * 128); // limit +/- 60 degrees around -90 degrees
    }
  }
  else
    arg_sh = 0;

  // set calculated results
  NVIC_DisableIRQ(TIM4_IRQn); // disable TIM4 interrupt first, because it can re-enable TIM1 interrupt
  NVIC_DisableIRQ(TIM1_UP_IRQn); // disable TIM1 interrupt, both because of "read-while-write" protection
    
  amplitude = mod;
  advance_sh = arg_sh;

  NVIC_EnableIRQ(TIM1_UP_IRQn); // enable interrupts again
  NVIC_EnableIRQ(TIM4_IRQn);
  
  // battery voltage notification when in idle (< 0 % -> 0 beeps, 0 - 19 % -> 1 beep, ..., > 80 % -> 5 beeps)
  if(run == FALSE)
  {
    if(++batt_cnt == 3000) // beep after 3 seconds in idle
    {
      for(beeps = 0; beeps < 5; beeps++)
      {
        if(adc.value[3] < (3435 + beeps * 93)) // 3435 ~ 22.2 V, 93 ~ 0.6 V
          break;
      }

      if(beep_cnt == 0) // don't interrupt ongoing notification
        beep_cnt = beeps * 1024;
    }
    else if(batt_cnt > 3000)
      batt_cnt = 3001; // overflow protection
  }
  else
    batt_cnt = 0;

  if(beep_cnt > 0)
  {
    beep_cnt--;
    if((beep_cnt & 511) == 511)
      GPIOA->ODR ^= GPIO_ODR_ODR15; // toggle beep
  }

#ifdef DEBUG
  // save calculated results and send them over UART2WiFi bridge
  (p_tx_data + i)->a = U_d;
  (p_tx_data + i)->b = U_q;
  (p_tx_data + i)->c = I_d;
  (p_tx_data + i)->d = I_q;

  if(++i == 100) // every 100 ms send whole packet
  {
    i = 0;

    if(DMA1->ISR & DMA_ISR_TCIF2) // transfer complete flag
    {
      DMA1->IFCR = DMA_IFCR_CTCIF2; // clear transfer complete flag

      DMA1_Channel2->CCR &= ~DMA_CCR2_EN; // disable DMA1 Channel 2
      DMA1_Channel2->CMAR = (uint32_t) p_tx_data;
      DMA1_Channel2->CNDTR = sizeof(tx_data_a);
      DMA1_Channel2->CCR |= DMA_CCR2_EN; // enable DMA1 Channel 2
    }

    if(p_tx_data == tx_data_a) // change buffers
      p_tx_data = tx_data_b;
    else
      p_tx_data = tx_data_a;
  }

  // process incoming data
  if(DMA1_Channel3->CNDTR != sizeof(rx_data)) // in case of freeze, reset DMA settings after 1 second
  {
    if(++rx_timeout == 1000)
    {
      rx_timeout = 0;
      DMA1_Channel3->CCR &= ~DMA_CCR3_EN; // disable DMA1 Channel 3
      DMA1_Channel3->CMAR = (uint32_t) &rx_data;
      DMA1_Channel3->CNDTR = sizeof(rx_data);
      DMA1_Channel3->CCR |= DMA_CCR3_EN; // enable DMA1 Channel 3
    }
  }
  else
    rx_timeout = 0;
  
  if(DMA1->ISR & DMA_ISR_TCIF3) // transfer complete flag
  {
    DMA1->IFCR = DMA_IFCR_CGIF3; // clear transfer complete flag

    DMA1_Channel3->CCR &= ~DMA_CCR3_EN; // disable DMA1 Channel 3

    kpd = rx_data.a;
    kid = rx_data.b;
    kpq = rx_data.c;
    kiq = rx_data.d;

    kp = rx_data.e;
    kd = rx_data.f;

    DMA1_Channel3->CMAR = (uint32_t) &rx_data;
    DMA1_Channel3->CNDTR = sizeof(rx_data);
    DMA1_Channel3->CCR |= DMA_CCR3_EN; // enable DMA1 Channel 3
  }
#endif // DEBUG
}

void timer4_init(void)
{
  // PB8 (TIM4 CH3, INPUT), PB7 (TIM4 CH2, INPUT), PB6 (TIM4 CH1, INPUT) are in the default state

  TIM4->PSC = 320 - 1; // prescaler, 10 us @ 32 MHz
  TIM4->ARR = 6250 - 1; // max. measured time 62.5 ms -> 16 rpm and more

  TIM4->CR2 |= TIM_CR2_TI1S; // CH1, CH2, CH3 inputs XORed to TI1
  TIM4->SMCR |= TIM_SMCR_TS_2 | TIM_SMCR_SMS_2; // TI1 edge detector, after change reset the timer (reset mode)
  TIM4->DIER |= TIM_DIER_CC1IE | TIM_DIER_UIE; // capture and update/overflow interrupt enable
  TIM4->SR |= TIM_SR_UIF; // force interrupt (initial commutation) after enabling irq
  TIM4->CCMR1 |= TIM_CCMR1_CC1S; // IC1 mapped on TRC
  TIM4->CCER |= TIM_CCER_CC1E; // input capture enable
  TIM4->CR1 |= TIM_CR1_URS | TIM_CR1_CEN; // only underflow/overflow generates update, TIM4 enable
}

void TIM4_IRQHandler(void)
{
  uint8_t hall, rotation;
  static uint8_t last_hall = 0;
  static uint8_t last_rotation = 0;
  static uint8_t wait_one_time;

  NVIC_DisableIRQ(TIM1_UP_IRQn); // disable TIM1 interrupt to calmly update all needed data (rotation, rotor_step_sh, rotor_position_sh)
  
  rotation = last_rotation; // initialization in case of no change (and due to compiler warning)

  hall = (GPIOB->IDR >> 6) & 0x07;
  if(hall == hall_forward_future_lut[last_hall] || last_hall == 0)
  {
    rotation = FORWARD;
    GPIOC->BSRR = GPIO_BSRR_BR15 | GPIO_BSRR_BS14; // set LED_A and reset LED_B
  }
  else if(hall == hall_reverse_future_lut[last_hall])
  {
    rotation = REVERSE;
    GPIOC->BSRR = GPIO_BSRR_BS15 | GPIO_BSRR_BR14; // set LED_B and reset LED_A
  }

  if(rotation != last_rotation || hall == last_hall)
  {
    last_rotation = rotation;
    rotor_step_sh = 0;
    wait_one_time = 1;
  }

  last_hall = hall;

  if(TIM4->SR & TIM_SR_UIF) // overflow interrupt
  {
    TIM4->SR &= ~TIM_SR_UIF; // clear interrupt flag
    rotor_step_sh = 0;
    wait_one_time = 1;
  }

  if(TIM4->SR & TIM_SR_CC1IF) // commutation interrupt
  { 
    if(wait_one_time == 1)
    {
      TIM4->SR &= ~TIM_SR_CC1IF; // clear interrupt flag
      wait_one_time = 0;
    }
    else
    {
      rotor_step_sh = (300 * 128) / TIM4->CCR1; // interrupt flag is cleared by reading the register
      if(rotation == REVERSE)
        rotor_step_sh *= -1;
    }
  }

  //if(rotation == FORWARD)
    rotor_position_sh = rotor_forward_position_lut[hall] * 128;
  //else
    //rotor_position_sh = rotor_reverse_position_lut[hall] * 128;
    
  rotor_position_sh += rotor_step_sh * 2; // compensation of delay caused by RC filters

  NVIC_EnableIRQ(TIM1_UP_IRQn); // data updated, enable TIM1 interrupt again
}

int main(void)
{
  clock_init();
  SysTick_Config(32000); // 32 MHz / 32000 -> 1 ms
  
  gpio_init();

  delay_ms(2000); // global delay for proper power-up

  adc_init();
  
#ifdef DEBUG
  uart_init();
  DMA1_Channel2->CMAR = (uint32_t) tx_data_b;
  DMA1_Channel2->CNDTR = sizeof(tx_data_b);
  DMA1_Channel2->CCR |= DMA_CCR2_EN; // enable DMA1 Channel 2 (for the first time)

  DMA1_Channel3->CMAR = (uint32_t) &rx_data;
  DMA1_Channel3->CNDTR = sizeof(rx_data);
  DMA1_Channel3->CCR |= DMA_CCR3_EN; // enable DMA1 Channel 3 (for the first time)
#endif // DEBUG

  spi_init();
  lsm6ds3_init();

  timer1_init();
  NVIC_SetPriority(TIM1_UP_IRQn, 1);

  timer2_init();
  NVIC_SetPriority(TIM2_IRQn, 3);

  timer4_init();
  NVIC_SetPriority(TIM4_IRQn, 2);

  NVIC_EnableIRQ(TIM1_UP_IRQn);
  NVIC_EnableIRQ(TIM2_IRQn);
  NVIC_EnableIRQ(TIM4_IRQn);

  while(1)
    ;
}
