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

void TIM6_DAC_IRQHandler();

//static uint8_t irs_update_flag = 0;
static uint8_t control_update_flag = 0;
static uint8_t allowed_calib = 0;

imu_raw_t imu_raw;
imu_scaled_t imu;
IRS irs;
attitude_t attitude;
rc_channels rx_chs_raw;
rc_channels rx_chs_normalized;
control_channels_t ctrl_chs;
//lidar_data lidar;

Vector3 euler;

void imu_callback(uint8_t* buf, uint8_t size);
void delay_ms(uint32_t ms);

int main(void)
{
  SystemClock_Config(); // 170 MHz
  
	__enable_irq();
	
	NVIC_SetPriority(TIM6_DAC_IRQn, 2);
	NVIC_SetPriority(USART3_IRQn, 1);
	NVIC_SetPriority(LPUART1_IRQn, 0);
	
	imu_pow_init();
	i2c_gpio_init();
	i2c1_init();
	imu_init();
	
	imu_processing_init();
	//imu_accel_calibrate();
	
	imu_tim6_init(500);
	
	IRS_init(&irs);
	
	attitude_init(&attitude);
	
	receiver_init();
	
	motors_init();
	
	while (1)
	{
	  receiver_update(&rx_chs_raw);
	  rx_chs_normalized = normalize_channels(rx_chs_raw);
		
		if (control_update_flag)
		{
      control_update_flag = 0;
      
      attitude_controller_update(
        &ctrl_chs,
        &rx_chs_normalized,
        &irs.q,
        &irs.gyro
      );
		}
		
		if (rx_chs_normalized.rc_armed)
	  {
      allowed_calib = 0;
      
      motors_set_throttle_mix(
        rx_chs_normalized.rc_throttle,
        &ctrl_chs,
        rx_chs_normalized.rc_armed
      );
	  }
	  else
		{
			motors_turn_off();
			allowed_calib = 1;
		}
	}
}

void imu_callback(uint8_t* buf, uint8_t size)
{
    imu_raw.ax = Rev16((buf[0] << 8) | buf[1]);
    imu_raw.ay = Rev16((buf[2] << 8) | buf[3]);
    imu_raw.az = Rev16((buf[4] << 8) | buf[5]);

    imu_raw.gx = Rev16((buf[6] << 8)  | buf[7]);
    imu_raw.gy = Rev16((buf[8] << 8)  | buf[9]);
    imu_raw.gz = Rev16((buf[10] << 8) | buf[11]);

    imu_process_raw(&imu, &imu_raw, &allowed_calib);

    irs.gyro.x = imu.gx;
    irs.gyro.y = imu.gy;
    irs.gyro.z = imu.gz;

    irs.accel.x = imu.ax;
    irs.accel.y = imu.ay;
    irs.accel.z = imu.az;

    IRS_update(&irs, IMU_DT);
    
    euler = QuatToEuler(&irs.q);
}

void TIM6_DAC_IRQHandler()
{
	if (TIM6->SR & TIM_SR_UIF)
	{
		TIM6->SR &= ~TIM_SR_UIF;
    
    imu_get_async(imu_callback);
		control_update_flag = 1;
	}
}

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