// sensors.hpp 시험 (tests/test_sensors_cpp.py 가 복셀 월드를 만들어 넘긴다): 평지(z 0) + x = 20 의 벽.
#include <cmath>
#include <cstdio>

#include "sensors.hpp"

using namespace vkgs;
#define CHECK(c)                                                    \
    do {                                                            \
        if (!(c)) { std::printf("실패 %s:%d %s\n", __FILE__, __LINE__, #c); return 1; } \
    } while (0)

int main(int, char** argv) {
    VoxelWorld w;
    w.load(argv[1]);
    // 1) 광선: 위에서 아래로 → 지면 칸 위쪽 면 (z 0 ~ 0.2 칸) 에 닿는다
    float o[3] = {5.1f, 0.1f, 3.0f}, down[3] = {0, 0, -1}, fwd[3] = {1, 0, 0};
    float t = w.raycast(o, down, 50);
    CHECK(t > 2.7f && t < 3.05f);
    float o2[3] = {5.1f, 0.1f, 1.0f};
    float tw = w.raycast(o2, fwd, 50);  // 벽 x = 20
    CHECK(std::abs(tw - (20.0f - 5.1f)) < 0.25f);
    // 2) 라이다: 지면 점은 z ≈ 0, 벽 점은 x ≈ 20, 아래 채널(−30.67°) 의 지면 거리 ≈ 1.84 / sin(30.67°)
    LidarConfig lc;
    lc.noise = 0;
    EgoState e;  // 원점, 동쪽(+x)
    auto pts = lidar_scan(w, {}, lc, e);
    CHECK(pts.size() > 20000);
    int ground = 0, wall = 0;
    float r0 = 0;
    int n0 = 0;
    for (const auto& p : pts) {
        if (std::abs(p.p[2] - 0.2f) < 0.25f) ++ground;
        if (std::abs(p.p[0] - 20.0f) < 0.3f && p.p[2] > 0.5f) ++wall;
        if (p.ring == 0) r0 += p.range, ++n0;
    }
    std::printf("lidar %zu점, 지면 %d, 벽 %d, 0번 링 평균 거리 %.2f (기대 %.2f)\n", pts.size(), ground, wall, r0 / n0,
                (1.84f - 0.1f) / std::sin(30.67f * 3.14159265f / 180.0f));
    CHECK(ground > 10000 && wall > 500);
    CHECK(std::abs(r0 / n0 - (1.84f - 0.1f) / std::sin(30.67f * 3.14159265f / 180.0f)) < 0.1f);
    // 3) 객체 상자가 벽보다 앞에 있으면 가린다
    std::vector<SensorBox> boxes{{10.0f, 0.0f, 0.2f, 0.0f, 2.0f, 1.0f, 1.6f, -5.0f, 0.0f}};  // x 10, 다가오는 차 (−5 m/s)
    auto pts2 = lidar_scan(w, boxes, lc, e);
    int on_box = 0;
    for (const auto& p : pts2) on_box += p.object == 0;
    CHECK(on_box > 50);
    // 4) 레이더 (앞): 자차 10 m/s 로 전진 — 정적 벽의 vr ≈ −10 cosθ, 다가오는 차는 vr ≈ −15
    RadarConfig rc;
    rc.mount = {"RADAR_FRONT", {3.4f, 0, 0.5f}, {0, 0, 0}};
    rc.noise_range = rc.noise_vel = 0;
    e.v = 10;
    auto det = radar_scan(w, boxes, rc, e);
    CHECK(!det.empty() && int(det.size()) <= rc.max_points);
    bool car = false, wall_ok = true;
    for (const auto& d : det) {
        if (d.object == 0 && std::abs(d.azimuth) < 3) car = car || std::abs(d.vr + 15.0f) < 0.5f;
        if (d.object < 0 && std::abs(d.azimuth) < 30) {
            float expect = -10.0f * d.s[0] / std::sqrt(d.s[0] * d.s[0] + d.s[1] * d.s[1] + d.s[2] * d.s[2]);
            wall_ok = wall_ok && std::abs(d.vr - expect) < 0.3f;
        }
        CHECK(d.object >= 0 || d.p[2] >= e.z + rc.min_height - 1e-3f);  // 노면 반사는 없다
    }
    std::printf("radar %zu점, 다가오는 차 vr −15 %s, 정적 반사 도플러 %s\n", det.size(), car ? "ok" : "없음", wall_ok ? "ok" : "어긋남");
    CHECK(car && wall_ok);
    std::printf("ok\n");
    return 0;
}