#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;

volatile uint32_t us_ticks = 0;

void SysTick_Handler()
{
	us_ticks++;
}

float micros()
{
	return (float)us_ticks;
}

static float last_time = 0;

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

int main(void)
{
  last_time = micros();
  
	__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 (irs_update_flag)
		{
		  irs_update_flag = 0;
			
			imu_read_scaled(&imu, &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);
		}*/
		
		
		
		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)
{
	imu_raw.ax = (buf[0] << 8) | buf[1];
	imu_raw.ay = (buf[2] << 8) | buf[3];
	imu_raw.az = (buf[4] << 8) | buf[5];

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

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

	float now = micros();
	float dt = (now - last_time) * 1e-6f;
  if (dt <= 0.0f || dt > 0.1f) dt = IMU_DT;
	last_time = now;

	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;
    i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
		control_update_flag = 1;
	}
}

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