#include "euler.h"
#include "dcm.h"
#include "helpler_funtions.h"
#include <math.h>

void Euler_ByDcmEnu(const float dcm[3][3], euler_angle_t *euler) {
  const float eps = 1e-4f;

  float phi_val = atan2f(-dcm[2][0], dcm[2][2]);
  float theta_val = asinf(dcm[2][1]);
  float psi_val = atan2f(-dcm[0][1], dcm[1][1]);
  const float pi = M_PI;

  /* 对 pitch 为 +-pi/2 的奇异点进行处理 */
  if ((fabsf(theta_val - pi / 2)) < eps) {
    /* 若 pitch 为 pi/2 */
    phi_val = 0;
    psi_val = atan2f(-dcm[0][2], -dcm[1][2]);

  } else if ((fabsf(theta_val + pi / 2)) < eps) {
    /* 若 pitch 为 -pi/2 */
    phi_val = 0;
    psi_val = atan2f(dcm[0][2], dcm[1][2]);
  }
  euler->roll = phi_val;
  euler->pitch = theta_val;
  euler->yaw = psi_val;
}

void Euler_ByDcmNed(const float dcm[3][3], euler_angle_t *euler) {
  const float eps = 1e-4f;

  float phi_val = atan2f(dcm[2][1], dcm[2][2]);
  float theta_val = asinf(-dcm[2][0]);
  float psi_val = atan2f(dcm[1][0], dcm[0][0]);
  const float pi = M_PI;

  /* 对 pitch 为 +-pi/2 的奇异点进行处理 */
  if ((fabsf(theta_val - pi / 2)) < eps) {
    /* 若 pitch 为 pi/2 */
    phi_val = (0);
    psi_val = (atan2f(dcm[1][2], dcm[0][2]));

  } else if ((fabsf(theta_val + pi / 2)) < eps) {
    /* 若 pitch 为 -pi/2 */
    phi_val = (0);
    psi_val = (atan2f(-dcm[1][2], -dcm[0][2]));
  }
  euler->roll = phi_val;
  euler->pitch = theta_val;
  euler->yaw = psi_val;
}

void Euler_ByQuaternionEnu(const float Q[4], euler_angle_t *euler) {
  float dcm[3][3] = {0};
  DCM_ByQuaternion(Q, dcm);
  Euler_ByDcmEnu(dcm, euler);
}

void Euler_ByQuaternionNed(const float Q[4], euler_angle_t *euler) {
  float dcm[3][3] = {0};
  DCM_ByQuaternion(Q, dcm);
  Euler_ByDcmNed(dcm, euler);
}