#pragma once
// math_tools.hpp
// 内联实现 solver.cpp 所需的全部数学工具函数，无外部依赖。

#include <cmath>
#include <Eigen/Dense>

namespace tools
{

// ──────────────────────────────────────────────────────────────────────────────
// square(x): 返回 x 的平方
// ──────────────────────────────────────────────────────────────────────────────
inline double square(double x) { return x * x; }

// ──────────────────────────────────────────────────────────────────────────────
// limit_rad(angle): 将弧度角限制在 (-π, π] 范围内
// ──────────────────────────────────────────────────────────────────────────────
inline double limit_rad(double angle)
{
  constexpr double two_pi = 2.0 * M_PI;
  angle = std::fmod(angle + M_PI, two_pi);
  if (angle < 0.0) angle += two_pi;
  return angle - M_PI;
}

// ──────────────────────────────────────────────────────────────────────────────
// eulers(R, ax0, ax1, ax2): 从旋转矩阵 R 提取欧拉角（内禀旋转，按 ax0->ax1->ax2 顺序）
//
// 常用调用方式：eulers(R, 2, 1, 0) 对应 ZYX（Yaw-Pitch-Roll）顺序。
// 返回 Vector3d { angle_ax0, angle_ax1, angle_ax2 }
// ──────────────────────────────────────────────────────────────────────────────
inline Eigen::Vector3d eulers(const Eigen::Matrix3d & R, int ax0, int ax1, int ax2)
{
  // 使用 Eigen 内置的欧拉角接口（内禀旋转）
  return R.eulerAngles(ax0, ax1, ax2);
}

// ──────────────────────────────────────────────────────────────────────────────
// xyz2ypd(xyz): 将三维空间坐标转换为 [yaw, pitch, distance]
//
// 坐标系约定（与云台/世界坐标系一致）：
//   x 轴：前方（射击方向）
//   y 轴：左方
//   z 轴：上方
//
// 返回 Vector3d { yaw, pitch, distance }（单位：弧度，弧度，米）
// ──────────────────────────────────────────────────────────────────────────────
inline Eigen::Vector3d xyz2ypd(const Eigen::Vector3d & xyz)
{
  double distance = xyz.norm();
  double yaw      = std::atan2(xyz.y(), xyz.x());
  double pitch    = std::atan2(xyz.z(), std::sqrt(xyz.x() * xyz.x() + xyz.y() * xyz.y()));
  return Eigen::Vector3d(yaw, pitch, distance);
}

// ──────────────────────────────────────────────────────────────────────────────
// get_abs_angle(a, b): 返回两个二维向量夹角的绝对值（弧度）
// ──────────────────────────────────────────────────────────────────────────────
inline double get_abs_angle(const Eigen::Vector2d & a, const Eigen::Vector2d & b)
{
  double cos_angle = a.normalized().dot(b.normalized());
  // 数值钳位避免 acos 域错误
  cos_angle = std::max(-1.0, std::min(1.0, cos_angle));
  return std::acos(cos_angle);
}

}  // namespace tools