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

#define RATE_LIM 300.0f

float angle_kp_pitch = 2.5f;
float angle_kp_roll  = 2.5f;
float angle_kp_yaw   = 2.0f;

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

void attitude_init(attitude_t* att)
{
    att->gyro = (Vector3){0};
}

void attitude_controller_update(control_channels_t* control,
                                const rc_channels* rx,
                                const Quaternion* current_q,
                                const Vector3* gyro)
{
    Quaternion q_target = rx_to_quaternion(rx);

    Quaternion q_error = QuatGetError(current_q, &q_target, true);

    Vector3 angle_error =
    {
        2.0f * q_error.x,
        2.0f * q_error.y,
        2.0f * q_error.z
    };

    Vector3 desired_rate =
    {
        angle_error.x * angle_kp_pitch,
        angle_error.y * angle_kp_roll,
        angle_error.z * angle_kp_yaw
    };

    desired_rate.x = constrain(desired_rate.x, -RATE_LIM, RATE_LIM);
    desired_rate.y = constrain(desired_rate.y, -RATE_LIM, RATE_LIM);
    desired_rate.z = constrain(desired_rate.z, -RATE_LIM, RATE_LIM);

    control->pitch = pid_update(&pid_pitch, desired_rate.x - gyro->x, gyro->x, IMU_DT);
    control->roll  = pid_update(&pid_roll,  desired_rate.y - gyro->y, gyro->y, IMU_DT);
    control->yaw   = pid_update(&pid_yaw,   desired_rate.z - gyro->z, gyro->z, IMU_DT);
}

Quaternion rx_to_quaternion(const rc_channels* rx)
{
	float pitch = int_mapping(rx->rc_pitch, -500, 500, -45, 45) * DEG2RAD;
	float roll 	= int_mapping(rx->rc_roll, -500, 500, -45, 45) * DEG2RAD;
	float yaw 	= 0;
	
	Vector3 pry = {pitch, roll, yaw};
	
	return QuatCreateFromEuler(&pry);
}

float constrain(float x, float min, float max)
{
	if (x < min) x = min; else if (x > max) x = max;
	return x;
}