#include "fgc/CaptureScheduler.h"

#include <chrono>

namespace fgc {

namespace {

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

// Whether a software trigger should fire given the current telemetry.
//
// NOTE: this preserves the original behaviour of triggering while the gimbal
// reports is_moving == 1. That looks counter-intuitive (one might expect to
// trigger once stopped). See docs/known-issues.md #7 - kept as-is pending
// confirmation of the firmware's is_moving semantics.
bool shouldTrigger(const MotorTelemetry& t) { return t.is_moving == 1; }

}  // namespace

CaptureScheduler::CaptureScheduler(IMotorController& motor, ICameraSource& camera,
                                   IControlChannel& channel, double image_rate,
                                   std::function<long long()> now_ms)
    : motor_(motor),
      camera_(camera),
      channel_(channel),
      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; }
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::tick() {
    // 1. Remote control input.
    ControlCommand cmd = channel_.poll();
    if (cmd.control_code_available) {
        control_code_ = cmd.control_code;
        channel_.publishStatus(control_code_);
    }
    if (cmd.heading_available) {
        target_heading_ = cmd.target_heading;
    }

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

    // 3. Capture cycle. The interval gate is 1000/rate milliseconds.
    if (control_code_ != 0 && control_code_ != 1) return;

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

    if (capture_active_ && interval_ms > 0.0 && elapsedMs() > interval_ms) {
        if (t.is_moving == 1) {
            resetTimer();
            trigger_after_stopping_ = true;
            if (control_code_ == 0)
                motor_.sendCommand("p");
            else  // control_code_ == 1
                motor_.sendCommand("kd" + target_heading_);
        }
    }

    if (trigger_after_stopping_ && elapsedMs() > 100) {
        if (shouldTrigger(t)) {
            if (camera_.trigger()) {
                trigger_after_stopping_ = false;
            }
        }
    }
}

}  // namespace fgc