#pragma once
#include <macros.h>
#include <vector>
#include <limits>
#include <math/eigen_alias.hpp>
#include <math/math_defs.hpp>
#include <primitive_descriptor.h>

using aabb_t = Eigen::AlignedBox<double, 3>;

namespace internal
{

template <int Dim>
using aabb_t_dim = Eigen::AlignedBox<double, Dim>;

constexpr double EPSILON = std::numeric_limits<double>::epsilon();

// 辅助函数：类型转换
namespace detail
{
static inline Eigen::Vector3d to_eigen(const vector3d& v) { return Eigen::Vector3d(v.x, v.y, v.z); }

} // namespace detail

// 辅助函数：坐标转换
namespace geometry_utils
{

/**
 * @brief 投影转换函数
 * @detail 将3D点投影到由 x_dir 和 y_dir 定义的平面上，并加上偏移量 offset，使得投影后的坐标系原点位于 offset 位置。
 * @param point 3D点坐标
 * @param x_dir 投影平面X轴方向
 * @param y_dir 投影平面Y轴方向
 * @param offset 投影后的偏移量
 * @return 2D投影坐标
 */
static inline auto manually_projection(const vector3d&                          point,
                                       const Eigen::Ref<const Eigen::Vector3d>& x_dir,
                                       const Eigen::Ref<const Eigen::Vector3d>& y_dir,
                                       const Eigen::Ref<const Eigen::Vector2d>& offset)
{
    Eigen::Vector3d p = Eigen::Map<const Eigen::Vector3d>(&point.x);
    return Eigen::Vector2d{p.dot(x_dir), p.dot(y_dir)} + offset;
}

static inline auto manually_projection(vector3d&&                               point,
                                       const Eigen::Ref<const Eigen::Vector3d>& x_dir,
                                       const Eigen::Ref<const Eigen::Vector3d>& y_dir,
                                       const Eigen::Ref<const Eigen::Vector2d>& offset)
{
    Eigen::Vector3d p = Eigen::Map<const Eigen::Vector3d>(&point.x);
    return Eigen::Vector2d{p.dot(x_dir), p.dot(y_dir)} + offset;
}
} // namespace geometry_utils

// ==================== Polyline 几何数据 ====================

/**
 * @brief Polyline 路径的几何计算数据
 * @details 用于构造拉伸体的 profile 和 axis，支持直线段和圆弧段
 */
EXTERN_C struct PE_API polyline_geometry_data {
    // ===== 几何数据 =====
    std::vector<Eigen::Vector2d> vertices;        // 所有顶点（包括圆弧的圆心和中间点）
    std::vector<uint8_t>         start_indices;   // 每个段的起始索引
    std::vector<double>          thetas;          // 每个段的角度（直线为0，圆弧为实际角度）
    bool                         is_axis = false; // 标记是否为 Axis

    // 最近点查询结果
    struct line_closest_param_t {
        Eigen::Vector3d point;         // 最近点的3D坐标
        double          t;             // 参数化坐标 [0, segment_count]
        bool            is_peak_value; // 是否是极值点（用于符号判定）
    };

    /**
     * @brief 作为 Axis 构建
     * @param desc Polyline 描述符
     * @param profile_ref_normal Profile 的参考法向量
     * @param axis_to_world 输出的坐标系变换矩阵
     * @param aabb 输出的3D AABB（世界坐标系）
     */
    void build_as_axis(const polyline_descriptor_t&                       desc,
                       const vector3d&                                    profile_ref_normal,
                       Eigen::Transform<double, 3, Eigen::AffineCompact>& axis_to_world,
                       aabb_t&                                            aabb);

    void build_as_axis(polyline_descriptor_t&&                            desc,
                       const vector3d&                                    profile_ref_normal,
                       Eigen::Transform<double, 3, Eigen::AffineCompact>& axis_to_world,
                       aabb_t&                                            aabb);

    /**
     * @brief 作为 Profile 构建
     * @param desc Polyline 描述符
     * @param proj_x_dir 投影平面X轴方向（世界坐标系）
     * @param proj_y_dir 投影平面Y轴方向（世界坐标系）
     * @param proj_origin 投影平面原点（世界坐标系）
     * @param aabb 输出的2D AABB（投影平面坐标系）
     */
    void build_as_profile(const polyline_descriptor_t& desc,
                          const Eigen::Vector3d&       proj_x_dir,
                          const Eigen::Vector3d&       proj_y_dir,
                          const Eigen::Vector3d&       proj_origin,
                          aabb_t_dim<2>&               aabb);

    void build_as_profile(polyline_descriptor_t&& desc,
                          const Eigen::Vector3d&  proj_x_dir,
                          const Eigen::Vector3d&  proj_y_dir,
                          const Eigen::Vector3d&  proj_origin,
                          aabb_t_dim<2>&          aabb);

    /**
     * @brief 计算参数 t 处的切线向量
     * @param t 参数化坐标 [0, segment_count]
     * @return 单位切线向量（2D）
     */
    [[nodiscard]] Eigen::Vector2d calculate_tangent(double t) const;

    /**
     * @brief 计算参数 t 处的法线向量（指向多段线内侧）
     * @details 对 CCW 缠绕的多段线，内法向定义为切向量的逆时针旋转 90°。
     *          直线段：inward = CCW_rot(tangent)
     *          圆弧段：通过顶点帧的叉积符号判断 bulge 正负，确保结果统一指向内侧。
     * @param t 参数化坐标 [0, segment_count]
     * @return 法线向量（2D，圆弧段可能未归一化）
     */
    [[nodiscard]] Eigen::Vector2d calculate_normal(double t) const;

    /**
     * @brief 计算点到 polyline 的最近点
     * @param p 查询点（3D坐标，z分量用于距离计算）
     * @return 最近点信息（位置、参数、是否极值）
     */
    [[nodiscard]] std::pair<line_closest_param_t, double> calculate_closest_param(
        const Eigen::Ref<const Eigen::Vector3d>& p) const;

    /**
     * @brief Point Membership Classification (PMC)
     * @details Determines which side of the polyline boundary a point lies on,
     *          using the INWARD normal returned by calculate_normal().
     *
     *          Let v = p - q  (from closest boundary point q toward query point p).
     *          v · n_inward < 0  →  p opposes inward direction  →  p is OUTSIDE  →  return true
     *          v · n_inward ≥ 0  →  p aligns with inward direction →  p is INSIDE →  return false
     *
     *          Convention (matches SDF sign rule):
     *            true  = point is OUTSIDE the polyline  (SDF > 0)
     *            false = point is INSIDE  the polyline  (SDF < 0)
     *
     * @param p             查询点（2D）
     * @param closest_param 最近点信息（由 calculate_closest_param 返回）
     * @return true  若点在 polyline 外部
     * @return false 若点在 polyline 内部
     */
    [[nodiscard]] bool pmc(const Eigen::Ref<const Eigen::Vector2d>& p, const line_closest_param_t& closest_param) const;

    /**
     * @brief 判断参数 t 是否在端点处
     * @param t 参数化坐标
     * @return true 如果在起点或终点
     */
    [[nodiscard]] bool isEnd(double t) const;

private:
    /**
     * @brief 内部通用构建逻辑
     * @param desc Polyline 描述符
     * @param project_func 投影函数（3D点 → 2D点）
     * @param aabb 2D AABB（可选）
     */
    void build_internal(const polyline_descriptor_t&                    desc,
                        std::function<Eigen::Vector2d(const vector3d&)> project_func,
                        aabb_t_dim<2>*                                  aabb = nullptr);
};

// ==================== Helixline 几何数据 ====================

/**
 * @brief Helixline（螺旋线）的几何计算数据
 * @details 用于螺旋拉伸体的 axis
 */
EXTERN_C struct PE_API helixline_geometry_data {
    // ===== 几何参数 =====
    double radius;         // 螺旋半径
    double height;         // 轴向高度
    double total_theta;    // 总旋转角度
    bool   is_righthanded; // 是否为右旋

    // ===== 最近点查询结果 =====
    struct line_closest_param_t {
        Eigen::Vector3d point;         // 最近点的3D坐标
        double          t;             // 参数化坐标
        bool            is_peak_value; // 是否是极值点
    };

    // ===== 核心方法 =====

    /**
     * @brief 从 descriptor 构建几何数据
     */
    void build_from_descriptor(const helixline_descriptor_t&                                        desc,
                               Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>& axis_to_world,
                               aabb_t&                                                              aabb);


    void build_from_descriptor(helixline_descriptor_t&&                                             desc,
                               Eigen::Transform<double, 3, Eigen::AffineCompact, Eigen::DontAlign>& axis_to_world,
                               aabb_t&                                                              aabb);

    /**
     * @brief 计算参数 t 处的切线向量
     * @param t 参数化坐标 [0, 1]
     * @return 单位切线向量（3D）
     */
    [[nodiscard]] Eigen::Vector3d calculate_tangent(double t) const;

    /**
     * @brief 计算参数 t 处的法线向量（径向）
     * @param t 参数化坐标 [0, 1]
     * @return 法线向量（3D）
     */
    [[nodiscard]] Eigen::Vector3d calculate_normal(double t) const;


    /**
     * @brief 计算点到螺旋线的最近点（使用峰值搜索 + 牛顿迭代）
     * @param p 查询点（3D坐标）
     * @return 最近点信息
     */
    [[nodiscard]] line_closest_param_t calculate_closest_param(const Eigen::Vector3d& p) const;

    /**
     * @brief 判断参数 t 是否在端点处
     */
    [[nodiscard]] bool isEnd(double t) const;
};

// ==================== TBN 坐标系构建 ====================

/**
 * @brief 构建 TBN 坐标系矩阵（复现旧架构）
 * @param tangent 切线方向（T）
 * @param normal 法线方向（N）
 * @param origin 坐标系原点
 * @return TBN 变换矩阵
 */

// 返回完整的 paired_model_matrix
inline paired_model_matrix build_TBN_paired_matrix(const Eigen::Vector3d& tangent,
                                                   const Eigen::Vector3d& normal,
                                                   const Eigen::Vector3d& origin)
{
    paired_model_matrix result;

    // 构建 local_to_world
    Eigen::Vector3d T = tangent.normalized();
    Eigen::Vector3d N = normal.normalized();
    Eigen::Vector3d B = N.cross(T).normalized();

    result.local_to_world.matrix().col(0).head<3>() = B;
    result.local_to_world.matrix().col(1).head<3>() = N;
    result.local_to_world.matrix().col(2).head<3>() = T;
    result.local_to_world.matrix().col(3).head<3>() = origin;
    // result.local_to_world.matrix()(3, 3)            = 1.0;

    // 自动计算逆矩阵
    result.world_to_local = result.local_to_world.inverse();

    return result;
}

// 针对 Polyline 的专用函数
inline paired_model_matrix build_polyline_cap_matrix(const Eigen::Vector3d& axis_point,
                                                     const Eigen::Vector3d& axis_tangent,
                                                     const Eigen::Vector3d& axis_normal,
                                                     bool                   is_top_cap = false)
{
    //Eigen::Vector3d tangent = is_top_cap ? -axis_tangent : axis_tangent;
    //return build_TBN_paired_matrix(tangent, axis_normal, axis_point);

    (void)is_top_cap; // both caps use the same +tangent direction; mark controls sign

    paired_model_matrix   result;
    const Eigen::Vector3d T      = axis_tangent.normalized();
    const Eigen::Vector3d N      = axis_normal.normalized();
    // Gram-Schmidt: make N orthogonal to T
    const Eigen::Vector3d N_orth = (N - N.dot(T) * T).normalized();
    const Eigen::Vector3d B_orth = T.cross(N_orth);

    result.local_to_world.matrix().col(0).head<3>() = T; // plane normal = axis tangent
    result.local_to_world.matrix().col(1).head<3>() = N_orth;
    result.local_to_world.matrix().col(2).head<3>() = B_orth;
    result.local_to_world.matrix().col(3).head<3>() = axis_point;
    result.world_to_local                           = result.local_to_world.inverse();
    return result;
}

} // namespace internal