#include "stm32f0xx.h"
#include <math.h>
#include "owi.h"
#include "adc.h"
#include "i2c.h"
//#include "uart.h"
#include "mpu6050.h"
#include "m24c01.h"
#include "ssd1306.h"
#include "utils.h"
#include "com_tables.h"

volatile uint16_t tick;
uint16_t power_cnt;
uint8_t text[10];

struct
{
  uint8_t step, last_step, direction;
} com; // commutation

struct
{
  uint16_t capture_reg;
  int8_t direction;
} rot; // rotation

struct
{
	struct
	{
		int16_t x, y, z;
	} accel;

	struct
	{
		int16_t x, y, z;
	} gyro;
} offset;

struct
{
  uint32_t trip_tick;
  uint16_t last_voltage, charge_cycles;
} eeprom;


void SysTick_Handler(void)
{
	tick++;
}

void clock_init(void)
{
	RCC->CR |= (1 << RCC_CR_HSEON_Pos); // enable HSE oscillator (8 MHz)
	while(!(RCC->CR & (1 << RCC_CR_HSERDY_Pos))) // wait for HSE oscillator to be ready
		;
	RCC->CFGR |= (2 << RCC_CFGR_PLLMUL_Pos)|(1 << RCC_CFGR_PLLSRC_Pos)|(2 << RCC_CFGR_SW_Pos); // PLL x 4, HSE selected as PLL input clock, HCLK and PCLK same as SYSCLK (32 MHz), PLL used as SYSCLK
	RCC->CFGR3 |= (1 << RCC_CFGR3_I2C1SW_Pos); // SYSCLK as I2C1 clock
	RCC->CR |= (1 << RCC_CR_PLLON_Pos); // enable PLL
	while(!(RCC->CR & (1 << RCC_CR_PLLRDY_Pos))) // wait for PLL to be ready
		;

	RCC->AHBENR |= (1 << RCC_AHBENR_GPIOBEN_Pos)|(1 << RCC_AHBENR_GPIOAEN_Pos); // enable GPIOB, GPIOA clock
	RCC->APB1ENR |= (1 << RCC_APB1ENR_I2C1EN_Pos)|(1 << RCC_APB1ENR_TIM14EN_Pos)|(1 << RCC_APB1ENR_TIM3EN_Pos); // enable I2C1, TIMER14, TIMER3 clock
	RCC->APB2ENR |= (1 << RCC_APB2ENR_TIM16EN_Pos)|(1 << RCC_APB2ENR_USART1EN_Pos)|(1 << RCC_APB2ENR_TIM1EN_Pos)|(1 << RCC_APB2ENR_ADCEN_Pos); // enable TIMER16, USART1, TIMER1, ADC clock
}

void gpio_init(void)
{
  GPIOB->MODER &= ~((3 << GPIO_MODER_MODER2_Pos)|(3 << GPIO_MODER_MODER8_Pos)); // PB2, PB8 input (not present in LQFP32 package)

	GPIOA->MODER |= (1 << GPIO_MODER_MODER1_Pos)|(1 << GPIO_MODER_MODER11_Pos); // PA1 output (on/off), PA11 output (buzzer)
	GPIOA->OSPEEDR |= (3 << GPIO_OSPEEDR_OSPEEDR1_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR11_Pos); // high speed

	GPIOB->MODER |= (1 << GPIO_MODER_MODER4_Pos)|(1 << GPIO_MODER_MODER5_Pos); // PB4 (led-b), PB5 (led-f) output
	GPIOB->OSPEEDR |= (3 << GPIO_OSPEEDR_OSPEEDR4_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR5_Pos); // high speed
	GPIOB->BSRR = GPIO_BSRR_BS_5;
}

void timer1_init(void)
{
	GPIOA->MODER |= (2 << GPIO_MODER_MODER7_Pos)|(2 << GPIO_MODER_MODER8_Pos)|(2 << GPIO_MODER_MODER9_Pos)|(2 << GPIO_MODER_MODER10_Pos); // PA7 (tim1 ch1n), PA8 (tim1 ch1), PA9 (tim1 ch2), PA10 (tim1 ch3) alternate function
	GPIOA->OSPEEDR |= (3 << GPIO_OSPEEDR_OSPEEDR8_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR8_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR9_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR10_Pos); // high speed
	GPIOA->AFR[0] |= (2 << GPIO_AFRL_AFRL7_Pos); // used by TIMER1
	GPIOA->AFR[1] |= (2 << GPIO_AFRH_AFRH0_Pos)|(2 << GPIO_AFRH_AFRH1_Pos)|(2 << GPIO_AFRH_AFRH2_Pos);

	GPIOB->MODER |= (2 << GPIO_MODER_MODER0_Pos)|(2 << GPIO_MODER_MODER1_Pos); // PB0 (tim1 ch2n), PB1 (tim1 ch3n) alternate function
	GPIOB->OSPEEDR |= (3 << GPIO_OSPEEDR_OSPEEDR0_Pos)|(3 << GPIO_OSPEEDR_OSPEEDR1_Pos); // high speed
	GPIOB->AFR[0] |= (2 << GPIO_AFRL_AFRL0_Pos)|(2 << GPIO_AFRL_AFRL1_Pos); // used by TIMER1

	TIM1->PSC = 3; // prescaler
	TIM1->ARR = 399; // 20 kHz @ 32 MHz
	TIM1->CCR1 = 399; // inverted 0% duty
	TIM1->CCR2 = 399;
	TIM1->CCR3 = 399;

	TIM1->BDTR |= (20 << TIM_BDTR_DTG_Pos)|(1 << TIM_BDTR_MOE_Pos); // 2.5 us deadtime, enable timer outputs
	TIM1->CR2 |= (1 << TIM_CR2_CCPC_Pos); // CCxE, CCxNE, OCxM bits preloaded
	TIM1->CR1 |= (2 << TIM_CR1_CKD_Pos)|(1 << TIM_CR1_CEN_Pos); // t_dts 32 MHz / 4 for deadtime, timer enable
}

void timer3_init(void)
{
  GPIOA->MODER |= (2 << GPIO_MODER_MODER6_Pos); // PA6 (tim3 ch1)
  GPIOA->AFR[0] |= (1 << GPIO_AFRL_AFRL6_Pos); // used by TIMER3

  TIM3->CCMR1 |= (16 << TIM_CCMR1_IC1F_Pos)|(1 << TIM_CCMR1_CC1S_Pos); // signal sampling 8 events @ t_dts * 32, IC1 mapped to TI1
  TIM3->SMCR |= (5 << TIM_SMCR_TS_Pos)|(4 << TIM_SMCR_SMS_Pos); // trigger input TI1FP1, reset the counter
  TIM3->CCER |= (1 << TIM_CCER_CC1NP_Pos)|(1 << TIM_CCER_CC1P_Pos)|(1 << TIM_CCER_CC1E_Pos); // both edges, capture enable

  TIM3->PSC = 255; // 125 kHz @ 32 MHz
  TIM3->SR &= ~((1 << TIM_SR_CC1IF_Pos)|(1 << TIM_SR_UIF_Pos)); // clear interrupts flags
  TIM3->DIER |= (1 << TIM_DIER_CC1IE_Pos)|(1 << TIM_DIER_UIE_Pos); // interrupts enable
  TIM3->CR1 |= (2 << TIM_CR1_CKD_Pos)|(1 << TIM_CR1_URS_Pos)|(1 << TIM_CR1_CEN_Pos); // t_dts 32 MHz / 4, overflow update source, timer enable
}

void TIM3_IRQHandler(void)
{
  static int8_t last_rot_dir = 0;
  static uint8_t discard;

  if(rot.direction != last_rot_dir)
  {
    discard = 1;
    last_rot_dir = rot.direction;
  }

  if(TIM3->SR & (1 << TIM_SR_CC1IF_Pos)) // input capture
  {
    if(discard) // discard the first value after direction change or overflow
    {
      TIM3->SR &= ~(1 << TIM_SR_CC1IF_Pos); // clear interrupt flag
      discard = 0;
      rot.capture_reg = 0;
    }
    else
      rot.capture_reg = TIM3->CCR1; // flag is cleared after reading the register
  }

  if(TIM3->SR & (1 << TIM_SR_UIF_Pos)) // counter overflow
  {
    TIM3->SR &= ~(1 << TIM_SR_UIF_Pos); // clear interrupt flag
    discard = 1;
    rot.capture_reg = 0;
  }
}

void timer16_init(void)
{
  TIM16->PSC = 31;
  TIM16->ARR = 999; // t_pulse
  TIM16->CCR1 = 999; // t_delay
  TIM16->CR1 |= (1 << TIM_CR1_OPM_Pos); // one pulse mode
}

void timer14_init(void)
{
	TIM14->PSC = 319; // prescaler
	TIM14->ARR = 499; // 200 Hz @ 32 MHz
	TIM14->SR &= ~(1 << TIM_SR_CC1IF_Pos); // clear interrupt flag
	TIM14->DIER |= (1 << TIM_DIER_CC1IE_Pos); // interrupt enable
	TIM14->CR1 |= (1 << TIM_CR1_CEN_Pos); // timer enable
}

void TIM14_IRQHandler(void)
{
	float gyro_pitch_angular_vel, gyro_pitch_angle; //, gyro_roll_angle, gyro_yaw_angle;
	static float pitch_angle = 0;
	//static float roll_angle = 0;
	uint16_t voltage, abs_output;
	int16_t velocity, output;
	static uint8_t run = 0;
	static float sum_pitch_angle = 0;
	static int16_t last_output = 0;
	static uint8_t lcd_info = 0;

	TIM14->SR &= ~(1 << TIM_SR_CC1IF_Pos); // clear interrupt flag

	mpu6050_read_data();

	accel.x -= offset.accel.x;
	accel.y -= offset.accel.y;
	accel.z -= offset.accel.z;

	gyro.x -= offset.gyro.x;
	gyro.y -= offset.gyro.y;
	gyro.z -= offset.gyro.z;

	gyro_pitch_angular_vel = gyro.y / 65.5f * M_PI / 180.0f;
	gyro_pitch_angle = gyro_pitch_angular_vel / 200.0f;
  //gyro_roll_angle = gyro.x / 65.5f * M_PI / 180.0f / 200.0f;
  //gyro_yaw_angle = -gyro.z / 65.5f * M_PI / 180.0f / 200.0f;

  //gyro_pitch_angle += gyro_yaw_angle * roll_angle;
  //gyro_roll_angle -= gyro_yaw_angle * pitch_angle;

  pitch_angle = 0.99f * (pitch_angle + gyro_pitch_angle) + 0.01f * atan2f((float) -accel.x, (float) accel.z);
  //roll_angle = 0.99f * (roll_angle + gyro_roll_angle) + 0.01f * atan2f((float) accel.y, (float) accel.z);

  voltage = adc_voltage();

  if(rot.capture_reg != 0 && rot.capture_reg >= 750) // max 500 rpm
    velocity = (rot.direction * 375000) / rot.capture_reg;
  else
    velocity = 0;

  if(run)
  {
    sum_pitch_angle += pitch_angle / 200.0f; // or * dt
    if(sum_pitch_angle > 2.0f)
      sum_pitch_angle = 2.0f;
    if(sum_pitch_angle < -2.0f)
      sum_pitch_angle = -2.0f;

    output = 4000 * pitch_angle + 1500 * sum_pitch_angle + 250 * gyro_pitch_angular_vel; // PID controller, P - 4000, I - 1500, D - 250
    output = (output * 2220) / voltage; // change PWM accordingly to supply voltage
    output += (velocity * 379) / 400; // bemf compensation

    if(output < 0)
    {
      com.direction = 0;
      abs_output = -output;
    }
    else
    {
      com.direction = 1;
      abs_output = output;
    }

    if(((output >= 0) - (output < 0)) != ((last_output >= 0) - (last_output < 0))) // has the sign/direction changed?
      EXTI->SWIER |= (1 << EXTI_SWIER_SWIER4_Pos); // force commutation
    last_output = output;

    if(abs_output > 379)
      abs_output = 379;

    if(GPIOB->IDR & GPIO_IDR_3) // log. 1 -> button released
    {
      TIM1->CCR1 = 399; // inverted 0% duty
      TIM1->CCR2 = 399;
      TIM1->CCR3 = 399;
      run = 0;
      sum_pitch_angle = 0;
      last_output = 0;
    }
    else // log. 0 -> button pressed
    {
      TIM1->CCR1 = 399 - abs_output;
      TIM1->CCR2 = 399 - abs_output;
      TIM1->CCR3 = 399 - abs_output;
    }
  }
	else
	{
	  if(!(GPIOB->IDR & GPIO_IDR_3)) // log. 0 -> button pressed
	  {
	    power_cnt = 0;
	    if((pitch_angle > -0.01f) && (pitch_angle < 0.01f)) // horizontal tilt achieved
	      run = 1;
	  }
	  else
	    power_cnt++;
	}

  ssd1306_goto_xy(0, lcd_info + 1);
  switch(lcd_info)
  {
    case 0: number2text(text, voltage, 4, 2);
            ssd1306_print((char *) text);
            ssd1306_print(" V");
            lcd_info = 1;
            break;

    case 1: number2text(text, eeprom.trip_tick / 707.35f, 7, 2);
            ssd1306_print((char *) text);
            ssd1306_print(" km");
            lcd_info = 2;
            break;

    case 2: number2text(text, eeprom.charge_cycles, 5, 0);
            ssd1306_print((char *) text);
            ssd1306_print(" nc");
            lcd_info = 0;
            break;

    default: break;
  }
}

void commutation(void)
{
  TIM1->CCER &= ~0x0555; // all outputs inactive
  TIM1->CCMR1 &= ~0x7070; // OC1M, OC2M frozen
  TIM1->CCMR2 &= ~0x0070; // OC3M frozen

  TIM1->CCER |= (com.direction) ? bldc_cw_ccer[com.step] : bldc_ccw_ccer[com.step];
  TIM1->CCMR1 |= (com.direction) ? bldc_cw_ccmr1[com.step] : bldc_ccw_ccmr1[com.step];
  TIM1->CCMR2 |= (com.direction) ? bldc_cw_ccmr2[com.step] : bldc_ccw_ccmr2[com.step];

  TIM1->EGR |= (1 << TIM_EGR_COMG_Pos); // write CCxE, CCxNE, OCxM bits at the same time
}

void exti_init(void)
{
	EXTI->RTSR |= (1 << EXTI_RTSR_TR4_Pos)|(1 << EXTI_RTSR_TR5_Pos)|(1 << EXTI_RTSR_TR6_Pos); // rising trigger
	EXTI->FTSR |= (1 << EXTI_FTSR_TR4_Pos)|(1 << EXTI_FTSR_TR5_Pos)|(1 << EXTI_FTSR_TR6_Pos); // falling trigger
	EXTI->IMR |= (1 << EXTI_IMR_MR4_Pos)|(1 << EXTI_IMR_MR5_Pos)|(1 << EXTI_IMR_MR6_Pos); // enable interrupts on lines 4, 5, 6
}

void EXTI4_15_IRQHandler(void)
{
  uint8_t step;
  int8_t result;

	if(EXTI->PR & (1 << EXTI_PR_PR4_Pos)) // clear interrupt flags
		EXTI->PR |= (1 << EXTI_PR_PR4_Pos);
	if(EXTI->PR & (1 << EXTI_PR_PR5_Pos))
		EXTI->PR |= (1 << EXTI_PR_PR5_Pos);
	if(EXTI->PR & (1 << EXTI_PR_PR6_Pos))
		EXTI->PR |= (1 << EXTI_PR_PR6_Pos);

	if(!(TIM16->CR1 & (1 << TIM_CR1_CEN_Pos))) // only one commutation within 1 ms
	{
	  step = (GPIOA->IDR & (GPIO_IDR_4 | GPIO_IDR_5 | GPIO_IDR_6)) >> 4;
	  if(step >= 1 && step <= 6)
	  {
	    com.step = step - 1;
      result = bldc_rot_lut[com.step] - bldc_rot_lut[com.last_step];
      com.last_step = com.step;

      if(result == -1 || result == 5) // clockwise
      {
        GPIOB->BSRR = GPIO_BSRR_BR_5 | GPIO_BSRR_BS_4; // led-b active
        rot.direction = 1;
      }

      if(result == 1 || result == -5) // counter clockwise
      {
        GPIOB->BSRR = GPIO_BSRR_BS_5 | GPIO_BSRR_BR_4; // led-f active
        rot.direction = -1;
      }

      commutation();
      TIM16->CR1 |= (1 << TIM_CR1_CEN_Pos);
      eeprom.trip_tick++;
	  }
	}
}

int main(void)
{
  uint8_t step;

	clock_init();
  SysTick_Config(32000); // 32 MHz / 32000 = 1 kHz -> 1 ms

  gpio_init();
  owi_init();
  if(owi_authorisation())
  {
    GPIOA->BSRR = GPIO_BSRR_BS_1 | GPIO_BSRR_BS_11; // switch on and beep
    delay_ms(200);
    GPIOA->BSRR = GPIO_BSRR_BR_11;
  }
  else
    while(1) // do nothing
      ;

  adc_init();
  i2c_init();
  //uart_init();

  mpu6050_init();
  m24c01_read(0, (uint8_t *) &eeprom, sizeof(eeprom));

  if(adc_voltage() - eeprom.last_voltage >= 180) // 60% change
    eeprom.charge_cycles++;

  ssd1306_init();
  ssd1306_clear();
  ssd1306_goto_xy(0, 0);
  ssd1306_print("Voz""\x85""tko ver. 2");

  offset.accel.x = 1100;
  offset.accel.y = -100;
  offset.accel.z = 650;

  offset.gyro.x = -150;
  offset.gyro.y = 30;
  offset.gyro.z = 20;

  timer1_init();

  step = (GPIOA->IDR & (GPIO_IDR_4 | GPIO_IDR_5 | GPIO_IDR_6)) >> 4;
  if(step >= 1 && step <= 6)
  {
    com.step = step - 1;
    com.last_step = com.step;
    commutation();
  }
  else
  {
    GPIOA->BSRR = GPIO_BSRR_BS_11; // beep
    delay_ms(1000);
    GPIOA->BSRR = GPIO_BSRR_BR_11;
    goto switch_off; // restart and wheel move needed, continues few lines below
  }

  timer16_init();

  exti_init();
  NVIC_SetPriority(EXTI4_15_IRQn, 0);

  timer3_init();
  NVIC_SetPriority(TIM3_IRQn, 1);

  timer14_init();
  NVIC_SetPriority(TIM14_IRQn, 1);

  NVIC_EnableIRQ(EXTI4_15_IRQn);
  NVIC_EnableIRQ(TIM3_IRQn);
  NVIC_EnableIRQ(TIM14_IRQn);

  while(power_cnt != 12000)
    ;

  NVIC_DisableIRQ(EXTI4_15_IRQn);
  NVIC_DisableIRQ(TIM3_IRQn);
  NVIC_DisableIRQ(TIM14_IRQn);

  switch_off:
  TIM1->BDTR &= ~(1 << TIM_BDTR_MOE_Pos); // disable timer1 outputs

  ssd1306_clear();

  eeprom.last_voltage = adc_voltage();
  m24c01_write(0, (uint8_t *) &eeprom, sizeof(eeprom));

  GPIOA->BSRR = GPIO_BSRR_BS_11; // beep and switch off
  delay_ms(200);
  GPIOA->BSRR = GPIO_BSRR_BR_1 | GPIO_BSRR_BR_11;

  while(1)
    ;
}
