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

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 delay_ms(uint32_t ms);

int main(void)
{
	__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_calibrate();
	
	imu_tim6_init();
	
	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);
		
		    Vector3 gyro = {imu.gx, imu.gy, imu.gz};
		    Vector3 accel = {imu.ax, imu.ay, imu.az};
		
		    IRS_update(&irs, &gyro, &accel, IMU_DT);
		}
		
		euler = QuatToEuler(&irs.q);
		
		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)
	    {
	        motors_set_throttle_mix(
	            rx_chs_normalized.rc_throttle,
	            &ctrl_chs,
	            rx_chs_normalized.rc_armed
	        );
	    }
	    else
		{
			motors_turn_off();
		}
	}
}

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

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