#include "io/gpr/GpsTrack.hpp"

#include <cmath>
#include <fstream>
#include <sstream>
#include <stdexcept>

namespace geopro::io::gpr {

namespace {
constexpr double kMetersPerDegLat = 111320.0;
constexpr double kPi = 3.14159265358979323846;

// 严格把整列解析为 double；失败返回 false（不抛，供逐行容错跳过）。
bool parseDouble(const std::string& s, double& out) {
  try {
    std::size_t used = 0;
    out = std::stod(s, &used);
    return used == s.size();
  } catch (...) {
    return false;
  }
}
}  // namespace

GpsTrack parseGps(const std::string& path) {
  std::ifstream f(path);
  if (!f) throw std::runtime_error("parseGps: 打不开 " + path);

  GpsTrack track;
  std::string line;
  while (std::getline(f, line)) {
    // 去掉可能的 \r（CRLF）。
    if (!line.empty() && line.back() == '\r') line.pop_back();

    std::istringstream tok(line);
    std::vector<std::string> cols;
    std::string c;
    while (tok >> c) cols.push_back(c);
    if (cols.size() < 7) continue;  // 列数不足

    GpsPt p;
    if (!parseDouble(cols[2], p.lat)) continue;  // 纬
    if (!parseDouble(cols[4], p.lon)) continue;  // 经
    if (!parseDouble(cols[6], p.elev)) continue;  // 高
    track.pts.push_back(p);
  }
  return track;
}

XY lonLatToLocalM(double lat, double lon, double lat0, double lon0) {
  XY xy;
  xy.x = (lon - lon0) * kMetersPerDegLat * std::cos(lat0 * kPi / 180.0);
  xy.y = (lat - lat0) * kMetersPerDegLat;
  return xy;
}

PosHeading interpAlongTrack(const std::vector<XY>& trackM, double frac) {
  PosHeading r;
  if (trackM.empty()) return r;
  if (trackM.size() == 1) {
    r.pos = trackM.front();
    return r;
  }

  // 各段长度 + 累积里程。
  const std::size_t n = trackM.size();
  std::vector<double> cum(n, 0.0);
  for (std::size_t i = 1; i < n; ++i) {
    const double dx = trackM[i].x - trackM[i - 1].x;
    const double dy = trackM[i].y - trackM[i - 1].y;
    cum[i] = cum[i - 1] + std::hypot(dx, dy);
  }
  const double total = cum.back();

  // 退化（零长轨迹）：返回起点，航向取首段方向或 (1,0)。
  if (total <= 0.0) {
    r.pos = trackM.front();
    return r;
  }

  if (frac <= 0.0) frac = 0.0;
  if (frac >= 1.0) frac = 1.0;
  const double target = frac * total;

  // 找包含 target 里程的段 [i-1, i]。
  std::size_t seg = 1;
  while (seg < n && cum[seg] < target) ++seg;
  if (seg >= n) seg = n - 1;

  const double segLen = cum[seg] - cum[seg - 1];
  const double localFrac = segLen > 0.0 ? (target - cum[seg - 1]) / segLen : 0.0;

  const XY& a = trackM[seg - 1];
  const XY& b = trackM[seg];
  r.pos.x = a.x + (b.x - a.x) * localFrac;
  r.pos.y = a.y + (b.y - a.y) * localFrac;

  double dx = b.x - a.x;
  double dy = b.y - a.y;
  const double len = std::hypot(dx, dy);
  if (len > 0.0) {
    r.hx = dx / len;
    r.hy = dy / len;
  }
  return r;
}

}  // namespace geopro::io::gpr