#include "fgc/ScanGrid.h"

#include "fgc/Geometry.h"

#include <fstream>
#include <set>
#include <sstream>
#include <stdexcept>
#include <string>

namespace fgc {

namespace {

std::string trim(const std::string& s) {
    size_t b = s.find_first_not_of(" \t\r\n");
    if (b == std::string::npos) return "";
    size_t e = s.find_last_not_of(" \t\r\n");
    return s.substr(b, e - b + 1);
}

}  // namespace

std::vector<ScanPoint> parseScanCsv(const std::string& text) {
    std::vector<ScanPoint> pts;
    std::istringstream in(text);
    std::string line;
    int lineno = 0;
    while (std::getline(in, line)) {
        ++lineno;
        std::string t = trim(line);
        if (t.empty() || t[0] == '#') continue;
        // "yaw_deg,pitch_deg", or "yaw_deg" alone for pitch 0 (a yaw-only tower).
        size_t comma = t.find(',');
        try {
            ScanPoint p;
            p.yaw_deg = std::stod(trim(t.substr(0, comma)));
            if (comma != std::string::npos) p.pitch_deg = std::stod(trim(t.substr(comma + 1)));
            pts.push_back(p);
        } catch (const std::exception&) {
            throw std::runtime_error("scan grid line " + std::to_string(lineno) +
                                     ": non-numeric coordinate in '" + t + "'");
        }
    }
    return pts;
}

std::size_t repeatedBearings(const std::vector<ScanPoint>& points) {
    std::set<double> seen;
    std::size_t      repeats = 0;
    for (const auto& p : points)
        if (!seen.insert(wrap360(p.yaw_deg)).second) ++repeats;
    return repeats;
}

ScanGrid ScanGrid::fromConfig(const ScanConfig& cfg) {
    // No grid_file, no grid. Generating a default sweep here would put a tower
    // on the air photographing bearings nobody picked, under a configuration
    // that shows no waypoints at all; refusing leaves auto-sweep off and says
    // exactly what is missing.
    if (cfg.grid_file.empty())
        throw std::runtime_error("no scan grid configured: set [Scan] grid_file to a CSV of "
                                 "'yaw_deg,pitch_deg' waypoints (see config/scan.csv)");
    std::ifstream f(cfg.grid_file);
    if (!f) throw std::runtime_error("cannot open scan grid file: " + cfg.grid_file);
    std::stringstream buf;
    buf << f.rdbuf();
    auto pts = parseScanCsv(buf.str());
    if (pts.empty()) throw std::runtime_error("scan grid file has no waypoints: " + cfg.grid_file);
    return ScanGrid(std::move(pts));
}

void ScanGrid::advance() {
    if (points_.empty()) return;
    // Wrap rather than reverse: every pass visits the waypoints in the same
    // order, at the cost of one long return slew back to the start.
    index_ = (index_ + 1) % points_.size();
}

}  // namespace fgc