#pragma once

#include <math.h>

#ifdef __cplusplus
extern "C" {
#endif

/**
 * @brief 通过四元数构造旋转矩阵
 *
 * @param Q 输入四元数
 * @param dcm 输出旋转矩阵
 */
static inline void DCM_ByQuaternion(const float Q[4], float dcm[3][3]) {
  float Q0 = Q[0], Q1 = Q[1], Q2 = Q[2], Q3 = Q[3];
  float Q0s = Q[0] * Q[0];
  float Q1s = Q[1] * Q[1];
  float Q2s = Q[2] * Q[2];
  float Q3s = Q[3] * Q[3];
  dcm[0][0] = Q0s + Q1s - Q2s - Q3s;
  dcm[0][1] = 2.0f * (Q1 * Q2 - Q0 * Q3);
  dcm[0][2] = 2.0f * (Q0 * Q2 + Q1 * Q3);
  dcm[1][0] = 2.0f * (Q1 * Q2 + Q0 * Q3);
  dcm[1][1] = Q0s - Q1s + Q2s - Q3s;
  dcm[1][2] = 2.0f * (Q2 * Q3 - Q0 * Q1);
  dcm[2][0] = 2.0f * (Q1 * Q3 - Q0 * Q2);
  dcm[2][1] = 2.0f * (Q0 * Q1 + Q2 * Q3);
  dcm[2][2] = Q0s - Q1s - Q2s + Q3s;
}

/**
 * @brief 通过欧拉角构造旋转矩阵
 *  欧拉角定义顺序必须为 航向->俯仰->横滚 rad, enu-rfu 框架, 右手定则
 * @param yaw 航向角, rad
 * @param pitch 俯仰角, rad
 * @param roll 横滚角, rad
 * @param dcm 旋转矩阵, TbN阵
 */
static inline void DCM_Enu_ByEuler(float yaw, float pitch, float roll,
                               float dcm[3][3]) {
  float cos_r = cosf(roll);
  float cos_p = cosf(pitch);
  float cos_y = cosf(yaw);
  float sin_r = sinf(roll);
  float sin_p = sinf(pitch);
  float sin_y = sinf(yaw);

  dcm[0][0] = cos_r * cos_y - sin_p * sin_r * sin_y;
  dcm[0][1] = -cos_p * sin_y;
  dcm[0][2] = sin_r * cos_y + cos_r * sin_p * sin_y;

  dcm[1][0] = cos_r * sin_y + sin_r * sin_p * cos_y;
  dcm[1][1] = cos_p * cos_y;
  dcm[1][2] = sin_r * sin_y - cos_r * sin_p * cos_y;

  dcm[2][0] = -sin_r * cos_p;
  dcm[2][1] = sin_p;
  dcm[2][2] = cos_r * cos_p;
}

/**
 * @brief 通过欧拉角构造旋转矩阵
 *  欧拉角定义顺序必须为 航向->俯仰->横滚 rad, ned-frd 框架, 右手定则
 * @param yaw 航向角 rad
 * @param pitch 俯仰角 rad
 * @param roll 横滚角 rad
 * @param dcm 旋转矩阵, TbN阵
 */
static inline void DCM_Ned_ByEuler(float yaw, float pitch, float roll,
                                float dcm[3][3]) {
  float cos_r = cosf(roll);
  float cos_p = cosf(pitch);
  float cos_y = cosf(yaw);
  float sin_r = sinf(roll);
  float sin_p = sinf(pitch);
  float sin_y = sinf(yaw);

  dcm[0][0] = cos_p * cos_y;
  dcm[0][1] = -cos_r * sin_y + sin_r * sin_p * cos_y;
  dcm[0][2] = sin_r * sin_y + cos_r * sin_p * cos_y;

  dcm[1][0] = cos_p * sin_y;
  dcm[1][1] = cos_r * cos_y + sin_r * sin_p * sin_y;
  dcm[1][2] = -sin_r * cos_y + cos_r * sin_p * sin_y;

  dcm[2][0] = -sin_p;
  dcm[2][1] = sin_r * cos_p;
  dcm[2][2] = cos_r * cos_p;
}

/**
 * @brief 使用 frame1 xyz轴向量 在 frame2 下的坐标，构建旋转矩阵
 *  x, y, z 必须为互相正交的单位向量
 * @param x frame1 x 轴在 frame2 系下的单位坐标
 * @param y frame1 y 轴在 frame2 系下的单位坐标
 * @param z frame1 z 轴在 frame2 系下的单位坐标
 */
static inline void DCM_ByXYZAxis(const float X[3], const float Y[3],
                                 const float Z[3], float dcm[3][3]) {
  for (int i = 0; i < 3; ++i) {
    dcm[i][0] = X[i];
    dcm[i][1] = Y[i];
    dcm[i][2] = Z[i];
  }
}

#ifdef __cplusplus
}
#endif