// closed-loop 서버 프로토콜(serve.hpp)·제어 환산(drive.hpp control_for, step_with_contact) 시험 — tests/test_serve_cpp.py 가
// 컴파일해 돌린다. 창·GPU 없이.
//   test_serve self                       자체 시험 (실패 수 = 종료 코드)
//   test_serve contact OBST.pgm DRIV.txt  표준 입력 줄마다 'x y yaw' → '장애물점수 연석점수' (Python geometry 와 대조)
#include "drive.hpp"
#include "serve.hpp"

#include <cmath>
#include <cstdio>
#include <iostream>
#include <string>

using namespace vkgs;

static int fails = 0;
#define CHECK(c, msg)                          \
    do {                                       \
        if (!(c)) {                            \
            std::printf("FAIL %s\n", msg);     \
            ++fails;                           \
        }                                      \
    } while (0)

static bool throws(const std::string& line) {
    try {
        parse_command(line);
    } catch (const std::exception&) {
        return true;
    }
    return false;
}

int self_test() {
    // 1) 명령 파서
    ServeCommand c = parse_command("step accel=1.5 delta=-0.1 dt=0.05 render=1");
    CHECK(c.name == "step", "name");
    CHECK(std::fabs(c.num("accel") - 1.5f) < 1e-6f && std::fabs(c.num("delta") + 0.1f) < 1e-6f, "numbers");
    CHECK(c.flag("render", false) && !c.flag("sensors", false), "flags");
    CHECK(std::fabs(c.num("throttle", 0.25f) - 0.25f) < 1e-6f, "default");
    CHECK(parse_command("").name.empty() && parse_command("   # 주석").name.empty(), "empty/comment");
    CHECK(throws("step accel"), "token without =");
    CHECK(throws("step =3"), "empty key");
    bool bad_num = false;
    try { parse_command("step dt=0.05x").num("dt"); } catch (const std::exception&) { bad_num = true; }
    CHECK(bad_num, "trailing garbage number");
    bool missing = false;
    try { parse_command("step").num("dt"); } catch (const std::exception&) { missing = true; }
    CHECK(missing, "missing key");
    bool bad_flag = false;
    try { parse_command("render sensors=yes").flag("sensors", false); } catch (const std::exception&) { bad_flag = true; }
    CHECK(bad_flag, "bad flag");
    CHECK(parse_command("reset x=1 x=2").num("x") == 2.0f, "last key wins");

    // 2) 상자 목록
    auto b = parse_boxes("1,2,0.5,2.2,0.9;3,4,0,1,1;");
    CHECK(b.size() == 2 && b[1][0] == 3.0f && b[0][3] == 2.2f, "boxes");
    CHECK(parse_boxes("").empty(), "no boxes");
    bool bad_box = false;
    try { parse_boxes("1,2,3"); } catch (const std::exception&) { bad_box = true; }
    CHECK(bad_box, "short box");

    // 3) 응답 줄
    std::string l = ServeReply().add("t", 0.5, 2).add("cams", "A,B").add("obs", "").add("n", 3).line();
    CHECK(l == "ok t=0.50 cams=A,B obs=- n=3\n", "reply line");
    CHECK(serve_error("a\nb") == "err a b\n", "error line");

    // 4) 가속·조향 명령 → 페달·핸들: 정상 상태에서 명령한 가속이 그대로 나온다
    Vehicle car;
    car.v = 5;
    for (float a : {1.5f, 0.0f, -2.0f, -6.5f}) {
        Vehicle t = car;
        float v0 = t.v;
        t.step(control_for(t, a, 0), 0.01f);
        float got = (t.v - v0) / 0.01f;
        std::printf("accel cmd %.2f -> %.3f\n", a, got);
        CHECK(std::fabs(got - a) < 0.01f, "accel tracking");
    }
    // 서 있을 때 음의 가속은 후진이 아니라 정지 유지
    Vehicle st;
    for (int i = 0; i < 100; ++i) st.step(control_for(st, -3.0f, 0), 0.01f);
    CHECK(std::fabs(st.v) < 1e-6f && std::fabs(st.x) < 1e-6f, "hold at standstill");
    // 조향각 명령: 조향 속도 제한(1.2 rad/s) 뒤 목표각에 수렴
    Vehicle s2;
    s2.v = 4;
    for (int i = 0; i < 100; ++i) s2.step(control_for(s2, 0.0f, 0.2f), 0.01f);
    std::printf("delta after 1 s: %.4f (cmd 0.2)\n", s2.delta);
    CHECK(std::fabs(s2.delta - 0.2f) < 2e-3f, "steer angle");

    // 5) step_with_contact: 벽(x = 10 m) 앞에서 멈추고 막힘을 알린다 (빈 지도·주행가능영역 = 막지 않음)
    ObstacleGrid none;
    Drivable road;
    ContactParams cp;
    cp.curb = false;
    Vehicle w;
    ContactInfo cur;
    int blocked = 0;
    for (int i = 0; i < 300; ++i) blocked += step_with_contact(w, control_for(w, 2.0f, 0), 0.01f, cur, none, road, cp).blocked;
    CHECK(blocked == 0 && w.x > 5.0f, "free driving");
    // 다른 차량 상자: 앞 8 m 에 정지 차량 → 막혀 멈춘다
    std::vector<Footprint> others{{w.x + 3.43f + 1.0f + 2.2f, w.y, 0, 2.2f, 0.9f}};
    int vb = 0;
    Contact kind = Contact::None;
    for (int i = 0; i < 300; ++i) {
        SubstepResult r = step_with_contact(w, control_for(w, 2.0f, 0), 0.01f, cur, none, road, cp, &others);
        if (r.blocked) ++vb, kind = r.kind;
    }
    std::printf("vehicle blocked steps %d, front %.2f, other rear %.2f\n", vb, w.x + w.geom.front, others[0].x - 2.2f);
    CHECK(vb > 0 && kind == Contact::Vehicle, "blocked by vehicle");
    CHECK(w.x + w.geom.front < others[0].x - 2.2f + 0.3f, "stopped at vehicle");
    return fails;
}

int contact_dump(const char* pgm, const char* driv) {
    ObstacleGrid og;
    og.load(pgm);
    Drivable road;
    road.load(driv);
    float x, y, yaw;
    while (std::cin >> x >> y >> yaw) {
        Vehicle car;
        car.x = x, car.y = y, car.yaw = yaw;
        ContactInfo ci = vehicle_contact(car, og, road, true, 0.15f, 0.1f);
        std::printf("%d %d\n", ci.obstacle, ci.curb);
    }
    return 0;
}

int main(int argc, char** argv) {
    std::string mode = argc > 1 ? argv[1] : "self";
    if (mode == "contact" && argc > 3) return contact_dump(argv[2], argv[3]);
    int f = self_test();
    std::printf("%s (%d)\n", f ? "FAILED" : "OK", f);
    return f;
}