#include <primitive/pattern/polyline_pattern.hpp>

namespace internal
{
std::pair<bool, Eigen::Vector2d> 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 auto      half_ab   = 0.5 * (a + b);
        const auto      bisec_vec = 0.5 * Eigen::Vector2d{a.y() - b.y(), b.x() - a.x()};
        const auto      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(); // 圆弧半径
        if (D < epsilon) return std::array{r, std::atan(bulge) * 2, a.x(), a.y()};

        // 计算GWN
        const auto term1  = std::atan(bulge);
        auto       dot    = ca.dot(cp);
        const auto cross1 = cp.x() * ca.y() - ca.x() * cp.y();
        const auto term2  = ((r - D) * cross1) / ((r + D) * (r * D + dot));
        dot               = cb.dot(cp);
        const auto cross2 = cp.x() * cb.y() - cb.x() * cp.y();
        const auto term3  = ((r - D) * cross2) / ((r + D) * (r * D + dot));
        const auto gwn    = term1 + term3 - term2;

        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,
        Eigen::Vector2d{x, y}
    };
}
} // namespace internal