#include "io/gpr/GpsTrack.hpp"

#include <gtest/gtest.h>

#include <cmath>
#include <cstdio>
#include <filesystem>
#include <fstream>

using namespace geopro::io::gpr;

// 经纬→米：纬 1° ≈ 111320 m；经按 cos(lat0) 缩。
TEST(GpsTrack, LonLatToLocalMatchesPhysics) {
  const double lat0 = 30.204, lon0 = 120.244;
  // 原点处归零。
  XY o = lonLatToLocalM(lat0, lon0, lat0, lon0);
  EXPECT_NEAR(o.x, 0.0, 1e-9);
  EXPECT_NEAR(o.y, 0.0, 1e-9);

  // 北移 0.001° 纬 → y ≈ 111.32 m，x≈0。
  XY north = lonLatToLocalM(lat0 + 0.001, lon0, lat0, lon0);
  EXPECT_NEAR(north.y, 111.32, 0.01);
  EXPECT_NEAR(north.x, 0.0, 1e-6);

  // 东移 0.001° 经 → x ≈ 111.32*cos(30.204°) ≈ 96.2 m。
  XY east = lonLatToLocalM(lat0, lon0 + 0.001, lat0, lon0);
  const double expX = 111.32 * std::cos(lat0 * 3.14159265358979323846 / 180.0);
  EXPECT_NEAR(east.x, expX, 0.01);
  EXPECT_NEAR(east.y, 0.0, 1e-6);
}

// 直线轨迹：frac=0/0.5/1 → 位置/航向正确。
TEST(GpsTrack, InterpStraightLine) {
  std::vector<XY> tr = {{0, 0}, {10, 0}, {20, 0}};  // 沿 +X 直线，长 20
  auto p0 = interpAlongTrack(tr, 0.0);
  EXPECT_NEAR(p0.pos.x, 0.0, 1e-9);
  EXPECT_NEAR(p0.hx, 1.0, 1e-9);
  EXPECT_NEAR(p0.hy, 0.0, 1e-9);

  auto pm = interpAlongTrack(tr, 0.5);
  EXPECT_NEAR(pm.pos.x, 10.0, 1e-9);
  EXPECT_NEAR(pm.pos.y, 0.0, 1e-9);

  auto p1 = interpAlongTrack(tr, 1.0);
  EXPECT_NEAR(p1.pos.x, 20.0, 1e-9);
  EXPECT_NEAR(p1.hx, 1.0, 1e-9);
}

// 折线（L 形）：里程一半落在拐角后，航向应是第二段方向。
TEST(GpsTrack, InterpPolyline) {
  std::vector<XY> tr = {{0, 0}, {10, 0}, {10, 10}};  // 先 +X 10，再 +Y 10，总长 20
  auto pm = interpAlongTrack(tr, 0.5);  // 里程 10 = 正好拐角
  EXPECT_NEAR(pm.pos.x, 10.0, 1e-9);
  EXPECT_NEAR(pm.pos.y, 0.0, 1e-9);

  auto p75 = interpAlongTrack(tr, 0.75);  // 里程 15 → 第二段中点 (10,5)
  EXPECT_NEAR(p75.pos.x, 10.0, 1e-9);
  EXPECT_NEAR(p75.pos.y, 5.0, 1e-9);
  EXPECT_NEAR(p75.hx, 0.0, 1e-9);  // 第二段沿 +Y
  EXPECT_NEAR(p75.hy, 1.0, 1e-9);
}

// 通道横向定位：航向 (hx,hy)，垂直 = (-hy,hx)；通道位置 = trace 位置 + o_c×垂直。
TEST(GpsTrack, ChannelLateralPlacement) {
  auto ph = interpAlongTrack({{0, 0}, {0, 10}}, 0.5);  // 沿 +Y，航向 (0,1)
  EXPECT_NEAR(ph.hx, 0.0, 1e-9);
  EXPECT_NEAR(ph.hy, 1.0, 1e-9);
  // 垂直(航向) = (-hy, hx) = (-1, 0)。偏移 +0.5 → 通道在 (-0.5, 5)。
  const double perpX = -ph.hy, perpY = ph.hx;
  const double oc = 0.5;
  const double cx = ph.pos.x + oc * perpX;
  const double cy = ph.pos.y + oc * perpY;
  EXPECT_NEAR(cx, -0.5, 1e-9);
  EXPECT_NEAR(cy, 5.0, 1e-9);
}

// 解析真实格式的 .gps（tab 分隔，含 N/E/M 标记列）。
TEST(GpsTrack, ParsesGpsFile) {
  auto tmp = std::filesystem::temp_directory_path() / "geopro_gps_test.gps";
  {
    std::ofstream f(tmp);
    f << "2023-06-03\t14:42:23:000\t30.21402519\tN\t120.24466077\tE\t9.390\tM\t4\r\n";
    f << "2023-06-03\t14:42:23:203\t30.21402388\tN\t120.24466074\tE\t9.386\tM\t4\r\n";
    f << "garbage line should be skipped\n";
  }
  GpsTrack t = parseGps(tmp.string());
  std::filesystem::remove(tmp);
  ASSERT_EQ(t.pts.size(), 2u);
  EXPECT_NEAR(t.pts[0].lat, 30.21402519, 1e-8);
  EXPECT_NEAR(t.pts[0].lon, 120.24466077, 1e-8);
  EXPECT_NEAR(t.pts[0].elev, 9.390, 1e-6);
}