#include "attitude.h"
#include "pid.h"
#include <math.h>

volatile uint8_t imu_update_flag = 0;
volatile uint8_t pid_update_flag = 0;

float angle_kp_roll = 2.5f;
float angle_kp_pitch = 2.5f;

pid_t pid_roll = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
pid_t pid_pitch = {.kp = 0.6f, .ki = 0.0f, .kd = 0.025f};
pid_t pid_yaw = {.kp = 1.8f, .ki = 0.6f, .kd = 0.0f};

int16_t desired_roll = 0;
int16_t desired_pitch = 0;

float desired_roll_rate = 0.0f;
float desired_pitch_rate = 0.0f;

float roll_rate_error = 0.0f;
float pitch_rate_error = 0.0f;
float yaw_rate_error = 0.0f;

float error_roll = 0.0f;
float error_pitch = 0.0f;
float error_yaw = 0.0f;

void complementary_filter_update(attitude_t* att, const imu_scaled_t* imu)
{
	static float roll_acc;
	static float pitch_acc;
	
	roll_acc = accel_roll_deg(imu);
	pitch_acc = accel_pitch_deg(imu);
	
	integrate_gyro_roll_deg(&att->roll, imu);
	integrate_gyro_pitch_deg(&att->pitch, imu);
	
	att->roll = CF_ALPHA * att->roll + (1 - CF_ALPHA) * roll_acc;
	att->pitch = CF_ALPHA * att->pitch + (1 - CF_ALPHA) * pitch_acc;
}

void attitude_update(attitude_t* attitude, imu_scaled_t* imu)
{
	if (imu_update_flag)
	{
		imu_update_flag = 0;
		
		imu_read_scaled(imu);
		complementary_filter_update(attitude, imu);
		yaw_rate_update(attitude, imu);
	}
}

void yaw_rate_update(attitude_t* attitude, imu_scaled_t* imu)
{
	attitude->yaw_rate = imu->gz;
}

void attitude_pid_update(control_channels_t* control, 
                         const rc_channels* rx, 
                         const attitude_t* att,
                         const imu_scaled_t* imu)
{
	if (pid_update_flag)
	{
		pid_update_flag = 0;
		
		desired_roll  = int_mapping(rx->rc_roll, -500, 500, -45, 45);
		desired_pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45);
		
		desired_roll_rate  = angle_kp_roll  * (desired_roll - att->roll);
		desired_pitch_rate = angle_kp_pitch * (desired_pitch - att->pitch);
		
		if (desired_roll_rate > 200) desired_roll_rate = 200;
		if (desired_roll_rate < -200) desired_roll_rate = -200;
		
		if (desired_pitch_rate > 200)  desired_pitch_rate = 200;
		if (desired_pitch_rate < -200) desired_pitch_rate = -200;
		
		roll_rate_error = desired_roll_rate - imu->gy;
		pitch_rate_error = desired_pitch_rate - imu->gx;
		yaw_rate_error = - imu->gz;
		
		control->roll  = pid_update(&pid_roll, roll_rate_error, imu->gy, IMU_DT);
		control->pitch = pid_update(&pid_pitch, pitch_rate_error, imu->gx, IMU_DT);
		control->yaw   = pid_update(&pid_yaw, yaw_rate_error, imu->gz, IMU_DT);
	}
}

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

float accel_roll_deg(const imu_scaled_t* imu) {
	// right-left
	return atan2f(imu->ax, sqrtf(imu->ay * imu->ay + imu->az * imu->az)) * 180.0f / PI;
}

float accel_pitch_deg(const imu_scaled_t* imu)
{
	// forward-backward
	return atan2f(-imu->ay, sqrtf(imu->ax * imu->ax + imu->az * imu->az)) * 180.0f / PI;
}

void integrate_gyro_roll_deg(float* roll, const imu_scaled_t* imu)
{
	// right-left
	*roll += imu->gy * IMU_DT;
}

void integrate_gyro_pitch_deg(float* pitch, const imu_scaled_t* imu)
{
	// forward-backward
	*pitch += imu->gx * IMU_DT;
}

void integrate_gyro_yaw_deg(float* yaw, const imu_scaled_t* imu)
{
	// forward-backward
	*yaw += imu->gz * IMU_DT;
}