#include "stm32g431xx.h"
#include "imu.h"
#include "imu_processing.h"
#include "timer.h"
#include "attitude.h"
#include "radio_receiver.h"
#include "motors.h"
#include "pid.h"


imu_scaled_t imu;
attitude_t attitude;
rc_channels rx_chs_raw;
rc_channels rx_chs_normalized;
control_channels_t ctrl_chs;

void delay_ms(uint32_t ms);

  int main(void)
{
  __enable_irq();
  
  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
  GPIOC->MODER &= ~(3 << (13 * 2));
  GPIOC->MODER |= 1 << (13 * 2);
  GPIOC->OTYPER &= ~(1 << 13);
  GPIOC->PUPDR &= ~(3 << (13 * 2));
  GPIOC->BSRR = 1 << (13 + 16);
  
  delay_ms(200);
  
  NVIC_SetPriority(TIM6_DAC_IRQn, 1);
  NVIC_SetPriority(LPUART1_IRQn, 0);
  
  i2c_gpio_init();
  i2c1_init();
  icm_init();
  imu_processing_init();
  tim6_init();
  
  imu_calibrate();
  
  receiver_init();
  
  motors_init();
  
  while (1)
  {
    attitude_update(&attitude, &imu);
    
    receiver_update(&rx_chs_raw);
    rx_chs_normalized = normalize_channels(rx_chs_raw);
    
    attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
    
    if (rx_chs_normalized.rc_armed)
    {
      motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);
    }
    else
    {
      motors_turn_off();
    }
  }
}

void delay_ms(uint32_t ms)
{
    for (uint32_t i = 0; i < ms * 4000; i++);
}