#include "fgc/CaptureScheduler.h"

#include "fgc/Logger.h"

#include <chrono>
#include <cmath>
#include <cstdlib>

namespace fgc {

namespace {

long long steadyNowMs() {
    using namespace std::chrono;
    return duration_cast<milliseconds>(steady_clock::now().time_since_epoch()).count();
}

// How close (encoder counts) an axis must be to its target, in addition to
// reporting standstill, to count as "settled". Generous vs. the firmware hold
// deadband (~200 counts).
constexpr long kSettleTolCounts = 600;

}  // namespace

CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
                                   IControlChannel& channel, double image_rate, Geometry geometry,
                                   ScanGrid& grid, std::function<long long()> now_ms)
    : motor_(motor),
      camera_(camera),
      channel_(channel),
      geometry_(geometry),
      grid_(grid),
      now_ms_(now_ms ? std::move(now_ms) : steadyNowMs),
      image_rate_(image_rate) {
    timer_start_ = now_ms_();
}

void CaptureScheduler::setCaptureActive(bool active) {
    capture_active_ = active;
    moving_ = false;  // restart the move/settle cycle cleanly
    resetTimer();
}
void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }

long long CaptureScheduler::elapsedMs() const { return now_ms_() - timer_start_; }
void      CaptureScheduler::resetTimer() { timer_start_ = now_ms_(); }

void CaptureScheduler::sendMove() {
    motor_.sendCommand("MOVE " + std::to_string(yaw_target_) + "," + std::to_string(pitch_target_));
}

bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
    return t.yaw.standstill && t.pitch.standstill &&
           std::labs(t.yaw.xenc - yaw_target_) <= kSettleTolCounts &&
           std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
}

void CaptureScheduler::tick() {
    // 1. Remote control input.
    ControlCommand cmd = channel_.poll();
    if (cmd.control_code_available) {
        control_code_ = cmd.control_code;
        LOG_TRACE_CAT(LogCat::Control) << "control_code -> " << control_code_;
        channel_.publishStatus(control_code_);
    }
    if (cmd.heading_available) {
        target_heading_ = cmd.target_heading;
        LOG_TRACE_CAT(LogCat::Control) << "target_heading -> " << target_heading_;
    }

    // 2. Telemetry.
    MotorTelemetry t = motor_.telemetry();

    // 3. Capture cycle. Only ControlCode 0 (sweep) and 1 (directed) act.
    if (control_code_ != 0 && control_code_ != 1) return;
    if (!capture_active_) return;

    const double interval_ms = image_rate_ > 0.0 ? 1000.0 / image_rate_ : 0.0;

    if (!moving_) {
        // Issue the next move once the interval has elapsed.
        if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;

        if (control_code_ == 0) {
            if (grid_.empty()) return;
            const ScanPoint& p = grid_.current();
            yaw_target_   = geometry_.yaw.toCounts(p.yaw_deg);
            pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
            LOG_TRACE_CAT(LogCat::Control)
                << "sweep -> grid yaw=" << p.yaw_deg << " pitch=" << p.pitch_deg;
        } else {  // ControlCode 1: directed yaw, pitch held at current position.
            try {
                yaw_target_ = geometry_.yaw.toCounts(std::stod(target_heading_));
            } catch (const std::exception&) {
                return;  // bad heading; wait for a valid one
            }
            pitch_target_ = t.pitch.xenc;
            LOG_TRACE_CAT(LogCat::Control) << "directed -> heading " << target_heading_;
        }
        sendMove();
        moving_ = true;
        resetTimer();
        return;
    }

    // Moving: trigger once both axes have settled at the target.
    if (settledAt(t)) {
        LOG_TRACE_CAT(LogCat::Control)
            << "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc << "; triggering";
        if (camera_.trigger()) {
            moving_ = false;
            if (control_code_ == 0) grid_.advance();
            resetTimer();
        }
    }
}

}  // namespace fgc