fwt_software/include/fgc/CaptureScheduler.h

#pragma once

#include "fgc/Geometry.h"
#include "fgc/ICameraSource.h"
#include "fgc/IMotorController.h"
#include "fgc/OperatingMode.h"
#include "fgc/ScanGrid.h"

#include <functional>
#include <string>
#include <vector>

namespace fgc {

// The capture control loop, expressed against the I/O interfaces so it can be
// unit-tested with mocks.
//
// Each tick():
//   1. reads motor telemetry (per-axis encoder counts + state),
//   2. runs the capture cycle. The interval is the time from one kept frame to
//      the next: as soon as a frame is kept it issues an absolute
//      `MOVE <yaw>,<pitch>` to the next target, waits until both axes report
//      standstill there, waits for the exposure to settle, then waits out what
//      is left of the interval and keeps the newest frame. If none is ready it
//      retries each tick, and gives up on the waypoint after frame_timeout_ms.
//
//      The exposure wait is settle_wait_ms, or with 0 automatic: until the camera
//      reports its auto-exposure on target or pinned at its limits
//      (ICameraSource::exposureState), at most frame_timeout_ms - after which the
//      frame is kept anyway, with a warning. A camera that cannot tell gets
//      kFallbackSettleMs.
//
//      When the move and the exposure wait alone take longer than the interval,
//      that waypoint warns - except the return from the last waypoint to the
//      first, and the first move after a mode starts, which are long by nature.
//
// What it captures is the mode (OperatingMode.h): auto sweeps the ScanGrid front
// to back, repeating; pan walks panHeadings() left to right at the pan's pitch,
// then jumps back to the left edge; manual captures nothing, bar a snapshot or
// continuous capture - both of which shoot wherever the operator has aimed.
// Degrees are converted to encoder counts via Geometry.
class CaptureScheduler {
public:
    // now_ms: monotonic millisecond clock; defaults to steady_clock. Injectable
    // for deterministic tests.
    CaptureScheduler(IMotorController& motor, ICameraSource& camera, double image_rate,
                     Geometry geometry, ScanGrid& grid, std::function<long long()> now_ms = {});

    // Switch mode. The caller (Application::setMode) has already checked that
    // the gimbal can do it; this restarts the move/settle cycle from the first
    // waypoint or pan position.
    void    setMode(Mode mode, const PanSpec& pan = {});
    Mode    mode() const { return mode_; }
    PanSpec pan() const { return pan_; }
    bool    captureActive() const { return mode_ != Mode::Manual; }

    // hardware.ini [Hardware] pan_step_deg: how much bearing one picture covers.
    void   setPanStep(double deg) { pan_step_deg_ = deg; }
    double panStep() const { return pan_step_deg_; }
    // The current pan's bearings, left to right, and the one being visited.
    const std::vector<double>& panHeadingsNow() const { return pan_headings_; }
    size_t panIndex() const { return pan_index_; }

    // Take one image now, outside any scan (the TUI's spacebar / `snapshot`).
    // Serviced on the next tick once the gimbal is standing still and the settle
    // wait has elapsed, so the frame is as clean as a scan frame. Ignored while a
    // capture is active. Idempotent: asking twice before it fires yields one image.
    void requestSnapshot();
    bool snapshotPending() const { return snapshot_pending_; }

    // Continuous capture (`capture on|off`): in manual, keep a frame on the scan's
    // own cycle - one per interval since the last, once standstill + the exposure
    // wait are done - wherever the operator has aimed. The gimbal stays free to
    // nudge; a frame is only kept at rest. No overrun warnings: the operator may be
    // jogging. Ignored outside manual, and cleared by any setMode().
    void setContinuous(bool on);
    bool continuous() const { return continuous_; }

    double imageRate() const { return image_rate_; }   // images per second

    // Per-camera-profile timing (see CameraProfile): how long to wait after standstill
    // for the exposure to settle (0 = automatic), and how long to keep retrying
    // before abandoning the waypoint. Called on the control thread, same as tick().
    void setSettleWait(int settle_ms, int timeout_ms);
    int  settleWaitMs() const { return settle_wait_ms_; }

    // Replace the degrees<->counts maps (e.g. after `gimbal heading fit`) so subsequent
    // scan waypoints target with the calibrated conversion. Called on the control
    // thread, same as tick(), so no locking is needed.
    void setGeometry(const Geometry& g) { geometry_ = g; }

    // Run one iteration of the control logic.
    void tick();

    // Inspection (mainly for tests).
    long        yawTargetCounts() const { return yaw_target_; }
    long        pitchTargetCounts() const { return pitch_target_; }
    bool        moving() const { return moving_; }
    // Where the current waypoint is: "moving", "settling" (exposure) or "waiting"
    // (for the rest of the interval). For the status line.
    const char* phase() const;
    // Waypoints that warned for running past the interval since construction.
    int         overruns() const { return overruns_; }

    // The fixed wait used when the settle wait is automatic but the camera
    // cannot report its exposure.
    static constexpr int kFallbackSettleMs = 1000;

private:
    // One frame's way to the camera, from standstill: when the gimbal came to
    // rest, when the exposure wait ended, when the trigger fell due (0 = not yet).
    struct Hold {
        long long settled_ms = 0;
        long long ready_ms   = 0;
        long long due_ms     = 0;
    };
    enum class Dwell { Waiting, Done, TimedOut };
    enum class Shot { Waiting, Captured, TimedOut };

    long long elapsedMs() const;
    void      resetTimer();
    void      sendMove();  // emit MOVE to (yaw_target_, pitch_target_)
    bool      settledAt(const MotorTelemetry& t) const;
    bool      startNextMove();   // pick the next target and MOVE; false if none
    void      finishWaypoint();
    std::string waypointLabel() const;   // "waypoint 3/15" / "pan position 2/6"
    void      serviceSnapshot(const MotorTelemetry& t);
    void      serviceContinuous(const MotorTelemetry& t);

    // Standstill seen: begin the exposure wait.
    void      beginHold(Hold& h);
    // Whether the exposure wait that began at h.settled_ms is over (sets
    // h.ready_ms). `what` names the frame in the warning when it times out.
    bool      dwellOver(Hold& h, const std::string& what);
    Dwell     dwell(long long settled_ms);
    // Trigger until a frame comes or frame_timeout_ms passes since it fell due.
    Shot      fire(Hold& h);

    // The part a snapshot and a continuous-capture frame share: wait for the
    // gimbal to stand still, then the exposure wait, then (continuous only) the
    // rest of the interval, then trigger until the frame budget runs out.
    Shot      shootAtRest(const MotorTelemetry& t, Hold& h, const std::string& what,
                          bool wait_interval);

    IMotorController& motor_;
    ICameraSource&    camera_;
    Geometry          geometry_;
    ScanGrid&         grid_;

    std::function<long long()> now_ms_;
    double      image_rate_;
    double      interval_ms_;           // 1000 / image_rate_; 0 = never
    long long   timer_start_ = 0;       // the last frame kept or given up, or the mode start

    Mode                mode_ = Mode::Manual;
    PanSpec             pan_;
    double              pan_step_deg_ = 6.0;
    std::vector<double> pan_headings_;
    size_t              pan_index_ = 0;

    bool        moving_        = false; // a MOVE has been issued, its frame not yet kept
    Hold        hold_;                  // the current waypoint's frame
    long long   move_sent_ms_  = 0;
    bool        first_move_    = true;  // no move yet since setMode()
    bool        quiet_overrun_ = false; // this waypoint is a long move by nature
    int         overruns_      = 0;
    bool        unknown_warned_ = false;  // the fallback has been announced this mode
    int         settle_wait_ms_   = 1000;
    int         frame_timeout_ms_ = 2000;
    bool        pitch_present_ = false; // a P: segment was seen in telemetry (2-axis rig)
    // One warning per run for each unreachable-heading case: a grid with a few
    // out-of-arc waypoints would otherwise log on every pass, forever.
    bool        unreachable_warned_     = false;
    bool        pan_unreachable_warned_ = false;

    bool        snapshot_pending_    = false;  // a manual snapshot is armed
    Hold        snapshot_hold_;
    bool        continuous_          = false;  // `capture on`
    Hold        continuous_hold_;
    long        yaw_target_    = 0;     // current target, encoder counts
    long        pitch_target_  = 0;
};

}  // namespace fgc