// filter.cpp - 点云 XY 过滤实现
#include "filter.h"
#include "config.h"
#include <cmath>

// mm，判定为原点的容差（与 config.h 中 ORIGIN_TOL_MM 一致，此处直接引用）
bool is_origin_point(double x_mm, double y_mm) {
    return std::abs(x_mm) < ORIGIN_TOL_MM && std::abs(y_mm) < ORIGIN_TOL_MM;
}

bool point_in_polygon(double px, double py,
                      const std::vector<std::pair<double, double>>& poly, int n)
{
    bool inside = false;
    for (int i = 0, j = n - 1; i < n; j = i++)
    {
        double xi = poly[i].first, yi = poly[i].second;
        double xj = poly[j].first, yj = poly[j].second;

        if (((yi > py) != (yj > py)) &&
            (px < (xj - xi) * (py - yi) / (yj - yi) + xi))
        {
            inside = !inside;
        }
    }
    return inside;
}

bool xy_filter_allowed(double x_mm, double y_mm) {
    if (is_origin_point(x_mm, y_mm)) return false;   // 原点噪点丢弃
    if (!FILTER_ENABLE) return true;                  // 未启用多边形过滤则全保留
    return point_in_polygon(x_mm, y_mm, radar_cfg().filter.polygon, radar_cfg().filter.poly_n);
}