#pragma once

#include "fgc/Geometry.h"
#include "fgc/ICameraSource.h"
#include "fgc/IMotorController.h"
#include "fgc/OperatingMode.h"
#include "fgc/ScanGrid.h"
#include "fgc/SettleRule.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 as a move -> settle -> wait -> keep machine: when
//      the interval elapses it issues an absolute `MOVE <yaw>,<pitch>` to the next
//      target, waits until both axes report standstill at that target, waits a
//      further settle_wait_ms so the free-running camera has exposed a frame with
//      the gimbal at rest, then keeps the newest frame. If none is ready it
//      retries each tick, and gives up on the waypoint after frame_timeout_ms.
//
// 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 - the interval, then standstill + settle wait - wherever the
    // operator has aimed. The gimbal stays free to nudge; a frame is only kept at
    // rest. 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

    // Is there room to save another frame (ImagePipeline::canAccept)? Asked
    // before moving to the next waypoint and before keeping a continuous-capture
    // frame. While it says no, the scan holds where it is, so a scan that
    // outruns saving slows down instead of losing pictures or filling memory.
    // A requested snapshot is never held. Unset: always room.
    void setSaveGate(std::function<bool()> can_save) { can_save_ = std::move(can_save); }
    // True while the scan is held for saving to catch up.
    bool holdingForSave() const { return holding_; }

    // Per-camera-profile timing (see CameraProfile): how long to wait after standstill
    // before keeping a frame, 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_; }

    // The keep rule for scan waypoints (profile setting settle_rule). Off: keep
    // the newest frame once settle_wait_ms has passed, as always. On: after
    // settle_wait_ms (now the mechanical minimum), wait until the camera's
    // frames say the image has settled (SettleRule), within frame_timeout_ms; a
    // camera that reports no frame data falls back to the fixed wait.
    void setSettleRule(bool on, const SettleRule::Params& params = {});
    bool settleRuleOn() const { return use_rule_; }

    // 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_; }

private:
    long long elapsedMs() const;
    void      resetTimer();
    void      sendMove();  // emit MOVE to (yaw_target_, pitch_target_)
    bool      settledAt(const MotorTelemetry& t) const;
    void      serviceSnapshot(const MotorTelemetry& t);
    void      serviceContinuous(const MotorTelemetry& t);

    // The part a snapshot and a continuous-capture frame share: wait for the
    // gimbal to stand still, then the settle wait, then trigger until the frame
    // budget runs out. `settled_ms` is that request's own settle clock.
    enum class Shot { Waiting, Captured, TimedOut };
    Shot      shootAtRest(const MotorTelemetry& t, long long& settled_ms);
    // The save gate, with one log line when a hold starts and one when it ends.
    bool      roomToSave();

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

    std::function<long long()> now_ms_;
    std::function<bool()>      can_save_;
    bool                       holding_ = false;
    double      image_rate_;
    long long   timer_start_ = 0;

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

    bool        use_rule_      = false; // keep by SettleRule, not after a fixed wait
    SettleRule  rule_;
    bool        rule_started_  = false; // this waypoint's rule has its first frame number
    uint64_t    rule_seq_      = 0;     // last frame fed to the rule
    bool        moving_        = false; // a MOVE has been issued, awaiting settle
    long long   settled_at_ms_ = 0;     // when standstill was first seen (0 = not yet)
    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
    long long   snapshot_settled_ms_ = 0;      // when standstill was first seen (0 = not yet)
    bool        continuous_          = false;  // `capture on`
    long long   continuous_settled_ms_ = 0;
    long        yaw_target_    = 0;     // current target, encoder counts
    long        pitch_target_  = 0;
};

}  // namespace fgc