#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 ping-pong (boustrophedon) cursor:
// the auto-sweep walks the list forward to the last point, then backward to the
// first, repeating - so a bounded (e.g. 180 deg) yaw arc never has to wrap.
//
// 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(); }
    const ScanPoint& current() const { return points_[index_]; }
    const std::vector<ScanPoint>& points() const { return points_; }

    // Step the cursor one waypoint, reversing direction at either end.
    void advance();
    void reset() { index_ = 0; forward_ = true; }

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

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

}  // namespace fgc