#include "Attitude.h"
#include "PID.h"
#include <math.h>

#define FREQUENCY     100.0f
#define PERIOD        1.0f / FREQUENCY

#define PI            3.14159265359f
#define DEG2RAD       PI / 180.0f
#define RATE_LIM      300.0f

constexpr float angle_kp_pitch = 2.5f;
constexpr float angle_kp_roll  = 2.5f;
constexpr 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){};
}

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 = current_q->GetError(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, PERIOD);
  control->roll  = pid_update(&pid_roll,  desired_rate.Y - gyro->Y, gyro->Y, PERIOD);
  control->yaw   = pid_update(&pid_yaw,   desired_rate.Z - gyro->Z, gyro->Z, PERIOD);
}

Quaternion rx_to_quaternion(const rc_channels* rx)
{
  Quaternion q;
	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};
	q.CreatePitchRollYaw(pry);
	return q;
}

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