// drive.hpp follow_path (자동 주행) — 직선 뒤 반지름 25 m 왼쪽 90° 커브를 따라가 끝에서 멈추는지
#include <cmath>
#include <cstdio>
#include <vector>

#include "drive.hpp"

int main() {
    std::vector<std::array<float, 2>> path;
    for (float x = 0; x < 40; x += 0.5f) path.push_back({x, 0});
    for (float a = 0; a <= 1.5708f; a += 0.02f) path.push_back({40 + 25 * std::sin(a), 25 - 25 * std::cos(a)});
    for (float y = 25; y < 60; y += 0.5f) path.push_back({65, y});
    vkgs::Vehicle car;
    size_t idx = 0;
    float worst = 0, vmax = 0;
    for (int k = 0; k < 4000; ++k) {  // 10 ms × 40 s
        vkgs::VehicleControl c = vkgs::follow_path(car, path, idx);
        car.step(c, 0.01f);
        if (k > 300) worst = std::max(worst, vkgs::distance_to_polyline(path, car.x, car.y));
        vmax = std::max(vmax, car.v);
    }
    float end = std::hypot(car.x - 65, car.y - 60);
    std::printf("worst %.2f m, vmax %.1f km/h, end %.1f m, v %.2f\n", worst, vmax * 3.6f, end, car.v);
    bool ok = worst < 0.6f && vmax > 15 / 3.6f && vmax < 21 / 3.6f && end < 9 && std::abs(car.v) < 0.1f;
    return ok ? 0 : 1;
}