#pragma once

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

#include <functional>
#include <string>

namespace fgc {

// The capture control loop, expressed against the I/O interfaces so it can be
// unit-tested with mocks.
//
// Each tick():
//   1. polls the control channel (updates control code / target heading,
//      echoes the code back as status),
//   2. reads motor telemetry (per-axis encoder counts + state),
//   3. 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.
//
// ControlCode 0 = automatic sweep through the ScanGrid (front to back, repeating);
// ControlCode 1 = drive yaw to the MQTT-supplied target heading (pitch held).
// 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, IControlChannel& channel,
                     double image_rate, Geometry geometry, ScanGrid& grid,
                     std::function<long long()> now_ms = {});

    void setCaptureActive(bool active);
    bool captureActive() const { return capture_active_; }

    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 calib`) 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).
    int         controlCode() const { return control_code_; }
    std::string targetHeading() const { return target_heading_; }
    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;

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

    std::function<long long()> now_ms_;
    double      image_rate_;
    long long   timer_start_ = 0;

    bool        capture_active_ = false;
    int         control_code_ = 0;
    std::string target_heading_ = "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)
    long        yaw_target_    = 0;     // current target, encoder counts
    long        pitch_target_  = 0;
};

}  // namespace fgc