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

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

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

    // 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);

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

    std::function<long long()> now_ms_;
    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        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)
    long        yaw_target_    = 0;     // current target, encoder counts
    long        pitch_target_  = 0;
};

}  // namespace fgc