#pragma once

#include "fgc/Config.h"

#include <cstddef>
#include <vector>

namespace fgc {

// One capture waypoint, in degrees (converted to encoder counts at command time
// via Geometry).
struct ScanPoint {
    double yaw_deg   = 0.0;
    double pitch_deg = 0.0;
};

// The ordered list of scan waypoints plus a one-way cursor: the auto-sweep walks
// the list front to back, then returns to the first point and repeats, so every
// pass covers the waypoints in the same order and each image sequence is
// directly comparable to the last.
//
// The cost is a long return slew from the last waypoint to the first, since the
// yaw arc is bounded and cannot wrap. Order the grid so that leg is acceptable.
//
// Points come either from an operator-edited CSV (ScanConfig::grid_file) or, if
// none is given, are generated from the [Scan] parameters.
class ScanGrid {
public:
    ScanGrid() = default;
    explicit ScanGrid(std::vector<ScanPoint> points) : points_(std::move(points)) {}

    // Build from config: load grid_file if set (throws std::runtime_error on a
    // missing/!malformed file), else generate from the parameters.
    static ScanGrid fromConfig(const ScanConfig& cfg);

    bool             empty() const { return points_.empty(); }
    std::size_t      size() const { return points_.size(); }
    std::size_t      index() const { return index_; }  // live cursor position
    const ScanPoint& current() const { return points_[index_]; }
    const std::vector<ScanPoint>& points() const { return points_; }

    // Step the cursor one waypoint, wrapping from the last back to the first.
    void advance();
    void reset() { index_ = 0; }

private:
    std::vector<ScanPoint> points_;
    std::size_t            index_ = 0;
};

// Exposed for unit testing.
std::vector<ScanPoint> parseScanCsv(const std::string& text);
std::vector<ScanPoint> generateScanGrid(const ScanConfig& cfg);

}  // namespace fgc