#include "fgc/ScanGrid.h"

#include <fstream>
#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);
}

std::vector<double> parseDoubleList(const std::string& csv) {
    std::vector<double> out;
    size_t start = 0;
    while (start <= csv.size()) {
        size_t comma = csv.find(',', start);
        std::string tok =
            trim(csv.substr(start, comma == std::string::npos ? std::string::npos : comma - start));
        if (!tok.empty()) out.push_back(std::stod(tok));
        if (comma == std::string::npos) break;
        start = comma + 1;
    }
    return out;
}

}  // 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;
        size_t comma = t.find(',');
        if (comma == std::string::npos)
            throw std::runtime_error("scan grid line " + std::to_string(lineno) +
                                     ": expected 'yaw_deg,pitch_deg', got '" + t + "'");
        try {
            ScanPoint p;
            p.yaw_deg   = std::stod(trim(t.substr(0, comma)));
            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::vector<ScanPoint> generateScanGrid(const ScanConfig& cfg) {
    std::vector<ScanPoint> pts;
    std::vector<double> pitches = parseDoubleList(cfg.pitch_levels);
    if (pitches.empty()) pitches.push_back(0.0);
    int n = cfg.yaw_intervals;
    if (n < 1) n = 1;
    // pitch-major, yaw inner (min -> max); the runtime cursor ping-pongs the whole list.
    for (double pitch : pitches) {
        for (int i = 0; i < n; ++i) {
            double frac = n > 1 ? static_cast<double>(i) / (n - 1) : 0.0;
            ScanPoint p;
            p.yaw_deg   = cfg.yaw_min_deg + frac * (cfg.yaw_max_deg - cfg.yaw_min_deg);
            p.pitch_deg = pitch;
            pts.push_back(p);
        }
    }
    return pts;
}

ScanGrid ScanGrid::fromConfig(const ScanConfig& cfg) {
    if (!cfg.grid_file.empty()) {
        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));
    }
    return ScanGrid(generateScanGrid(cfg));
}

void ScanGrid::advance() {
    if (points_.size() <= 1) return;
    if (forward_) {
        if (index_ + 1 < points_.size()) {
            ++index_;
        } else {
            forward_ = false;
            --index_;
        }
    } else {
        if (index_ > 0) {
            --index_;
        } else {
            forward_ = true;
            ++index_;
        }
    }
}

}  // namespace fgc