#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 -> trigger 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, then
//      software-triggers the cameras.
//
// ControlCode 0 = automatic sweep through the ScanGrid (ping-pong);
// 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_; }

    // 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        yaw_target_   = 0;      // current target, encoder counts
    long        pitch_target_ = 0;
};

}  // namespace fgc