#include <primitive/pattern/polyline_pattern.hpp>

namespace internal
{
double polyline_pattern::eval_sdf(Eigen::Vector2d p) const
{
    auto line_cpm = [&](auto index) {
        const auto            a        = this->vertices[index];
        const auto            b        = this->vertices[index + 1];
        const Eigen::Vector2d line_vec = b - a;

        const Eigen::Vector2d ap = p - a;
        const Eigen::Vector2d bp = p - b;

        // 计算投影参数
        const auto            t    = std::clamp(line_vec.dot(ap) / line_vec.squaredNorm(), .0, 1.);
        const Eigen::Vector2d proj = a + t * line_vec;
        const auto            dist = (p - proj).norm();

        // 计算GWN
        const auto dot     = ap.dot(bp);
        const auto cross   = ap.x() * bp.y() - ap.y() * bp.x();
        const auto d_theta = std::atan2(cross, dot);

        return std::array{dist, d_theta};
    };

    auto circle_cpm = [&](auto index) {
        const auto            bulge     = this->bulges[index];
        const auto            a         = this->vertices[index];
        const auto            b         = this->vertices[index + 1];
        const Eigen::Vector2d half_ab   = 0.5 * (a + b);
        const Eigen::Vector2d bisec_vec = 0.5 * Eigen::Vector2d{a.y() - b.y(), b.x() - a.x()};
        const Eigen::Vector2d center    = half_ab + bisec_vec / bulge;
        Eigen::Vector2d       ca        = a - center;
        Eigen::Vector2d       cb        = b - center;
        Eigen::Vector2d       cp        = p - center;
        Eigen::Vector2d       pa        = a - p;
        Eigen::Vector2d       pb        = b - p;

        const auto D          = cp.norm(); // 查询点到圆心的距离
        const auto r          = ca.norm(); // 圆弧半径
        const auto half_theta = std::atan(bulge);
        if (D <= epsilon) return std::array{r, half_theta * 2};

        // 计算GWN
        const auto gwn_main = half_theta;
        const auto r_mul_D = r * D, r_plus_D = r + D, abs_r_sub_D = std::abs(r - D);
        auto       gwn = gwn_main;
        if (abs_r_sub_D > epsilon) {
            auto dot   = ca.dot(cp);
            auto cross = cp.x() * ca.y() - ca.x() * cp.y();
            auto Y = r_plus_D * cross, X = abs_r_sub_D * (r_mul_D + dot);
            auto Ga = std::atan2(Y, X);
            if (std::abs(X) <= epsilon && std::abs(Y) <= epsilon) Ga = std::copysign(pi_div_2, -half_theta);
            dot     = cb.dot(cp);
            cross   = cp.x() * cb.y() - cb.x() * cp.y();
            Y       = r_plus_D * cross;
            X       = abs_r_sub_D * (r_mul_D + dot);
            auto Gb = std::atan2(Y, X);
            if (std::abs(X) <= epsilon && std::abs(Y) <= epsilon) Gb = std::copysign(pi_div_2, -half_theta);
            auto gwn_fix = Gb - Ga;
            if (gwn_fix * half_theta < 0) gwn_fix += std::copysign(pi, half_theta);
            gwn += (r > D) ? gwn_fix : -gwn_fix;
        }

        return std::array{std::min({abs_r_sub_D, pa.norm(), pb.norm()}), gwn};
    };

    // 单次遍历
    std::vector<size_t> indices(this->bulges.size());
    std::iota(indices.begin(), indices.end(), 0);
    auto [min_dist, gwn] = std::transform_reduce(
        indices.begin(),
        indices.end(),
        std::array{std::numeric_limits<double>::max(), .0},
        [](auto&& lhs, auto&& rhs) {
            const auto gwn  = lhs[1] + rhs[1];
            const auto dist = std::min(lhs[0], rhs[0]);
            return std::array{dist, gwn};
        },
        [&](size_t i) { return (std::abs(this->bulges[i]) <= epsilon) ? line_cpm(i) : circle_cpm(i); });

    // if (p.norm() > 1 && std::abs(gwn) / two_pi != .0) abort();
    return std::abs(gwn) < pi ? min_dist : -min_dist;
}

// double polyline_pattern::eval_sdf(Eigen::Vector2d p) const
// {
//     auto line_cpm = [&](auto index) {
//         const auto            a        = this->vertices[index];
//         const auto            b        = this->vertices[index + 1];
//         const Eigen::Vector2d line_vec = b - a;

//         const Eigen::Vector2d ap = p - a;
//         const Eigen::Vector2d bp = p - b;

//         // 计算投影参数
//         const auto            t    = std::clamp(line_vec.dot(ap) / line_vec.squaredNorm(), .0, 1.);
//         const Eigen::Vector2d proj = a + t * line_vec;
//         const auto            dist = (p - proj).norm();

//         // 计算GWN
//         const auto dot     = ap.dot(bp);
//         const auto cross   = ap.x() * bp.y() - ap.y() * bp.x();
//         const auto d_theta = std::atan2(cross, dot);

//         return std::array{dist, d_theta, proj.x(), proj.y()};
//     };

//     auto circle_cpm = [&](auto index) {
//         const auto            bulge     = this->bulges[index];
//         const auto            a         = this->vertices[index];
//         const auto            b         = this->vertices[index + 1];
//         const Eigen::Vector2d half_ab   = 0.5 * (a + b);
//         const Eigen::Vector2d bisec_vec = 0.5 * Eigen::Vector2d{a.y() - b.y(), b.x() - a.x()};
//         const Eigen::Vector2d center    = half_ab + bisec_vec / bulge;
//         Eigen::Vector2d       ca        = a - center;
//         Eigen::Vector2d       cb        = b - center;
//         Eigen::Vector2d       cp        = p - center;
//         Eigen::Vector2d       pa        = a - p;
//         Eigen::Vector2d       pb        = b - p;

//         const auto D          = cp.norm(); // 查询点到圆心的距离
//         const auto r          = ca.norm(); // 圆弧半径
//         const auto half_theta = std::atan(bulge);
//         if (D < epsilon) return std::array{r, half_theta * 2, a.x(), a.y()};

//         // 计算GWN
//         const auto gwn_main = half_theta;
//         const auto r_mul_D = r * D, r_plus_D = r + D, abs_r_sub_D = std::abs(r - D);
//         const auto cross1 = cp.x() * ca.y() - ca.x() * cp.y();
//         const auto cross2 = cp.x() * cb.y() - cb.x() * cp.y();
//         auto       gwn    = gwn_main;
//         if (abs_r_sub_D > epsilon) {
//             auto       dot     = ca.dot(cp);
//             const auto Ga      = std::atan2(r_plus_D * cross1, abs_r_sub_D * (r_mul_D + dot));
//             dot                = cb.dot(cp);
//             const auto Gb      = std::atan2(r_plus_D * cross2, abs_r_sub_D * (r_mul_D + dot));
//             auto       gwn_fix = Gb - Ga;
//             if (gwn_fix * half_theta < 0) gwn_fix += std::copysign(pi, half_theta);
//             gwn += (r > D) ? gwn_fix : -gwn_fix;
//         }

//         cp /= D;
//         ca /= r;
//         cb /= r;

//         // 原理：根据叉乘符号判断方向，不论何种情况cp都应该在ca和cb之间
//         if (cross1 * cross2 > 0) {
//             // 在圆弧范围内
//             return std::array{D - r, gwn, center.x() + cp.x(), center.y() + cp.y()};
//         } else {
//             // 最近点为圆弧端点
//             if (auto pa_norm = pa.norm(), pb_norm = pb.norm(); pa_norm < pb_norm)
//                 return std::array{pa_norm, gwn, a.x(), a.y()};
//             else
//                 return std::array{pa_norm, gwn, b.x(), b.y()};
//         }
//     };

//     // 单次遍历
//     std::vector<size_t> indices(this->bulges.size());
//     std::iota(indices.begin(), indices.end(), 0);
//     auto [min_dist, gwn, x, y] = std::transform_reduce(
//         indices.begin(),
//         indices.end(),
//         std::array{std::numeric_limits<double>::max(), .0, .0, .0},
//         [](auto&& lhs, auto&& rhs) {
//             const auto gwn = lhs[1] + rhs[1];
//             if (lhs[0] < rhs[0])
//                 return std::array{lhs[0], gwn, lhs[2], lhs[3]};
//             else
//                 return std::array{rhs[0], gwn, rhs[2], rhs[3]};
//         },
//         [&](size_t i) { return (std::abs(this->bulges[i]) <= epsilon) ? line_cpm(i) : circle_cpm(i); });

//     return {
//         // std::abs(gwn) < two_pi * epsilon,
//         std::abs(gwn) == 0.,
//         Eigen::Vector2d{x, y}
//     };
// }
} // namespace internal