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


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

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_tim6_init();
	imu_processing_init();
	
	imu_calibrate();
	
	receiver_init();
	
	motors_init();*/
	
	lidar_init();
	lidar_tim7_init();
	
	while (1)
	{
		lidar_update(&lidar);
	}
	
	/*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);
		
		lidar_update(&lidar);
		
		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++);
}