// drive.hpp 의 충돌·후진 회귀 시험 — tests/test_drive_cpp.py 가 컴파일해 돌린다.
#include "drive.hpp"
#include <cmath>
#include <cstdio>
using namespace vkgs;
int main(int, char** argv) {
    ObstacleGrid og; og.load(argv[1]); Drivable road;
    int fails = 0;
    // 1) 출발 자세가 기둥에 걸침: 그 자리에서 앞으로(빠져나가는 방향) 가면 막히지 않고, 벽(x=10)에서는 멈춘다
    Vehicle car; car.x = 0; car.y = 0.5f; car.yaw = 0;
    ContactInfo cur = vehicle_contact(car, og, road, false);
    std::printf("start contact %d\n", cur.total());
    // 전제: 정말로 기둥에 걸친 채 출발해야 '빠져나가기' 시나리오가 시험된다
    if (cur.total() <= 0) { std::puts("FAIL start pose does not overlap the pillar"); ++fails; }
    VehicleControl go; go.throttle = 1;
    float maxx = 0; int blocked = 0;
    for (int i = 0; i < 2000; ++i) {
        Vehicle prev = car; car.step(go, 0.01f);
        ContactInfo hit = vehicle_contact(car, og, road, false);
        if (hit.total() > cur.total()) { car = prev; car.v = 0; ++blocked; } else cur = hit;
        maxx = std::max(maxx, car.x + car.geom.front);
    }
    std::printf("front reached x=%.2f (wall at 10.0), blocked steps %d\n", maxx, blocked);
    // 허용치: obst_tol 0.1 + 샘플 간격 0.1
    if (maxx > 10.25f || maxx < 9.8f) { std::puts("FAIL wall"); ++fails; }
    // 2) 후진: 정지 상태에서 브레이크 5초 → -3 m/s 근처, 수 m 이동
    Vehicle r; VehicleControl back; back.brake = 1;
    for (int i = 0; i < 500; ++i) r.step(back, 0.01f);
    std::printf("reverse v=%.2f x=%.2f\n", r.v, r.x);
    if (r.v > -2.9f || r.x > -5.0f) { std::puts("FAIL reverse"); ++fails; }
    // 3) 후진 중 위 키 = 브레이크 → 0 에서 멈췄다가 전진
    VehicleControl fwd; fwd.throttle = 1; int k = 0;
    while (r.v < 0 && k < 300) { r.step(fwd, 0.01f); ++k; }
    std::printf("stopped after %.2f s, v=%.2f\n", k * 0.01f, r.v);
    if (k > 100) { std::puts("FAIL brake-from-reverse"); ++fails; }
    // 4) 기본 치수 = Zoe
    VehicleGeom g; std::printf("zoe geom rear %.2f front %.2f center %.2f\n", g.rear, g.front, g.center());
    // drive.hpp 의 기본값 (Renault Zoe: 축거 2.588 m, 뒤 오버행 0.65, 길이 4.08 → 앞 3.43, 폭 1.74)
    if (std::fabs(g.wheelbase - 2.588f) > 1e-4f || std::fabs(g.rear + 0.65f) > 1e-4f ||
        std::fabs(g.front - 3.43f) > 1e-4f || std::fabs(g.half_width - 0.87f) > 1e-4f ||
        std::fabs(g.center() - 0.5f * (g.front + g.rear)) > 1e-5f) {
        std::puts("FAIL zoe geom");
        ++fails;
    }
    // 5) 다른 차량(장면 객체 바닥 사각형): 앞 차에 막히고, 90° 돌아간 차는 폭 쪽에서 막히고, 옆 차선 차는 지나간다
    auto drive_into = [&](const std::vector<Footprint>& others) {
        Vehicle c; ObstacleGrid none; Drivable nr;
        ContactInfo cc = vehicle_contact(c, none, nr, false, 0.15f, 0.1f, &others);
        float mx = 0;
        for (int i = 0; i < 1500; ++i) {
            Vehicle prev = c; c.step(go, 0.01f);
            ContactInfo h = vehicle_contact(c, none, nr, false, 0.15f, 0.1f, &others);
            if (h.total() > cc.total()) { c = prev; c.v = 0; if (h.kind != Contact::Vehicle) return -1.0f; } else cc = h;
            mx = std::max(mx, c.x + c.geom.front);
        }
        return mx;
    };
    float f1 = drive_into({Footprint{12, 0, 0, 2.0f, 1.0f}});           // 뒤끝 x = 10
    float f2 = drive_into({Footprint{12, 0, 1.5707963f, 2.0f, 1.0f}});  // 옆으로 선 차: 가까운 면 x = 11
    float f3 = drive_into({Footprint{12, 3.0f, 0, 2.0f, 1.0f}});        // 옆 차선 (y = 3)
    std::printf("vehicle ahead stop %.2f (10), sideways %.2f (11), next lane %.2f (free)\n", f1, f2, f3);
    if (f1 > 10.25f || f1 < 9.8f) { std::puts("FAIL vehicle ahead"); ++fails; }
    if (f2 > 11.25f || f2 < 10.8f) { std::puts("FAIL vehicle sideways"); ++fails; }
    if (f3 < 30.0f) { std::puts("FAIL next lane blocked"); ++fails; }
    // 6) 비스듬히(17°) 벽에 박아 막힌 뒤 후진으로 빠져나온다 — 점 수만 비교하면 후진 스텝마다 +1 로 막혀 갇혔다 (2026-10-09)
    {
        Vehicle c; c.x = 3.0f; c.y = 4.0f; c.yaw = 0.3f;
        ContactInfo cc = vehicle_contact(c, og, road, false);
        auto step_blocked = [&](const VehicleControl& ctl) {
            Vehicle prev = c; c.step(ctl, 0.01f);
            ContactInfo h = vehicle_contact(c, og, road, false);
            float dir = c.v > 0 ? 1.0f : c.v < 0 ? -1.0f : 0.0f;
            if (h.depth(2) > cc.depth(2) && !escaping(c, dir, cc.depth(2), og, road, false, 0.15f, 0.1f, 2)) {
                c = prev; c.v = 0; return true;
            }
            cc = h; return false;
        };
        int blocked = 0;
        for (int i = 0; i < 600; ++i) blocked += step_blocked(go);
        float x_hit = c.x;
        VehicleControl back; back.brake = 1;
        for (int i = 0; i < 300; ++i) step_blocked(back);
        std::printf("angled wall: stopped x=%.2f (blocked %d, contact %d), after 3 s reverse x=%.2f\n", x_hit, blocked, cc.total(), c.x);
        if (blocked == 0 || x_hit + c.geom.front * std::cos(0.3f) > 10.4f) { std::puts("FAIL angled wall not blocking"); ++fails; }
        if (c.x > x_hit - 1.0f) { std::puts("FAIL stuck after angled hit (reverse)"); ++fails; }
    }
    // 7) 외톨이 한 칸이 차체 왼쪽 가장자리만 스친다 (샘플 2점 = graze): 막히지 않고 지나간다. graze 0 이면 막힌다
    for (int gz : {2, 0}) {
        Vehicle c; c.x = -16.0f; c.y = 4.28f; c.yaw = 0;  // 왼쪽 끝 샘플 y = 4.28 + 0.77 = 5.05 (칸 5.0~5.2), 그 안쪽 줄 4.95 는 밖
        ContactInfo cc = vehicle_contact(c, og, road, false);
        int blocked = 0;
        for (int i = 0; i < 400; ++i) {
            Vehicle prev = c; c.step(go, 0.01f);
            ContactInfo h = vehicle_contact(c, og, road, false);
            float dir = c.v > 0 ? 1.0f : -1.0f;
            if (h.depth(gz) > cc.depth(gz) && !escaping(c, dir, cc.depth(gz), og, road, false, 0.15f, 0.1f, gz)) {
                c = prev; c.v = 0; ++blocked;
            } else cc = h;
        }
        std::printf("graze %d: lone cell beside the car, x=%.2f blocked %d\n", gz, c.x, blocked);
        if (gz == 2 && (blocked || c.x < -5.0f)) { std::puts("FAIL grazing a lone cell blocks the car"); ++fails; }
        if (gz == 0 && !blocked) { std::puts("FAIL test setup: the lone cell does not touch the car"); ++fails; }
    }
    return fails;
}