#include "fgc/CaptureScheduler.h"

#include "fgc/Logger.h"

#include <chrono>
#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
    settled_at_ms_ = 0;
    // Drop any armed snapshot: a scan starting means the operator moved on, and
    // keeping it would fire an image at some unrelated later moment when the scan
    // stops - attributed to whatever the gimbal happened to be pointing at then.
    snapshot_pending_ = false;
    resetTimer();
}
void CaptureScheduler::setImageRate(double rate) { image_rate_ = rate; }

void CaptureScheduler::setSettleWait(int settle_ms, int timeout_ms) {
    settle_wait_ms_   = settle_ms < 0 ? 0 : settle_ms;
    frame_timeout_ms_ = timeout_ms < 0 ? 0 : timeout_ms;
}

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

void CaptureScheduler::sendMove() {
    // On a 1-axis (yaw-only) rig the firmware rejects the two-axis `MOVE y,p`
    // form ("ERR axis not present") and moves nothing, so address yaw alone.
    if (pitch_present_) {
        motor_.sendCommand("MOVE " + std::to_string(yaw_target_) + "," +
                           std::to_string(pitch_target_));
    } else {
        motor_.sendCommand("MOVE Y " + std::to_string(yaw_target_));
    }
}

bool CaptureScheduler::settledAt(const MotorTelemetry& t) const {
    bool yaw_ok = t.yaw.standstill && std::labs(t.yaw.xenc - yaw_target_) <= kSettleTolCounts;
    // A 1-axis rig never reports pitch standstill, so don't gate the capture on it.
    if (!t.pitch_present) return yaw_ok;
    return yaw_ok && t.pitch.standstill &&
           std::labs(t.pitch.xenc - pitch_target_) <= kSettleTolCounts;
}

void CaptureScheduler::requestSnapshot() {
    snapshot_pending_    = true;
    snapshot_settled_ms_ = 0;
}

// One frame, on request, through exactly the path a scan waypoint takes: the
// same standstill + settle wait, the same camera trigger, and therefore the same
// .jxl on disk and the same CamEvent on MQTT. Reusing the path rather than
// triggering the camera directly is the point - a snapshot that differed from a
// scan frame would be useless as a reference for aiming.
void CaptureScheduler::serviceSnapshot(const MotorTelemetry& t) {
    const bool standstill = t.yaw.standstill && (!pitch_present_ || t.pitch.standstill);
    if (!standstill) {
        snapshot_settled_ms_ = 0;  // still slewing (e.g. a jog is in flight)
        return;
    }
    if (snapshot_settled_ms_ == 0) {
        snapshot_settled_ms_ = now_ms_();
        return;
    }
    const long long since_settled = now_ms_() - snapshot_settled_ms_;
    if (since_settled < settle_wait_ms_) return;

    if (camera_.trigger()) {
        LOG_INFO << "snapshot captured";
        snapshot_pending_ = false;
        return;
    }
    // No fresh frame yet. Give up on the same budget a waypoint gets - measured
    // from settling, as the message says, not from the keypress: the operator may
    // have pressed Space mid-slew, and counting the slew against the camera would
    // abandon a snapshot the camera never had a chance at.
    if (since_settled > settle_wait_ms_ + frame_timeout_ms_) {
        LOG_WARN << "snapshot: no frame within " << frame_timeout_ms_
                 << " ms of settling; press Space again";
        snapshot_pending_ = false;
    }
}

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();
    pitch_present_ = t.pitch_present;

    // 3. Manual snapshot. Only outside a scan, so a keypress can never inject a
    //    frame into a running sweep or fight it for the settle window.
    if (!capture_active_) {
        if (snapshot_pending_) serviceSnapshot(t);
        return;
    }

    // 4. Capture cycle. Only ControlCode 0 (sweep) and 1 (directed) act.
    if (control_code_ != 0 && control_code_ != 1) 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 travel is under a full turn, so some bearings are unreachable.
            // Skip such a waypoint rather than clamping to the nearest limit,
            // which would image a direction nobody asked for and label it with
            // the one they did.
            auto yc = geometry_.yaw.headingToCounts(p.yaw_deg);
            if (!yc) {
                if (!unreachable_warned_) {
                    unreachable_warned_ = true;
                    LOG_WARN << "scan waypoint " << (grid_.index() + 1) << "/" << grid_.size()
                             << " heading " << p.yaw_deg
                             << " deg is outside the reachable yaw arc; skipping it (further "
                                "unreachable waypoints are skipped silently)";
                }
                grid_.advance();
                resetTimer();
                return;
            }
            yaw_target_   = *yc;
            pitch_target_ = geometry_.pitch.toCounts(p.pitch_deg);
            // One INFO line per move so the operator can follow the scan in the
            // log without enabling the control wire-trace.
            LOG_INFO << "scan move " << (grid_.index() + 1) << "/" << grid_.size()
                     << ": yaw " << p.yaw_deg << " pitch " << p.pitch_deg << " (deg)";
        } else {  // ControlCode 1: directed yaw, pitch held at current position.
            double heading = 0.0;
            try {
                heading = std::stod(target_heading_);
            } catch (const std::exception&) {
                return;  // bad heading; wait for a valid one
            }
            auto yc = geometry_.yaw.headingToCounts(heading);
            if (!yc) {
                // Hold position and say so: silently pointing at the nearest limit
                // would publish CamEvents claiming a bearing we cannot reach.
                if (!directed_unreachable_warned_) {
                    directed_unreachable_warned_ = true;
                    LOG_WARN << "target heading " << heading
                             << " deg is outside the reachable yaw arc; holding position";
                }
                return;
            }
            directed_unreachable_warned_ = false;
            yaw_target_   = *yc;
            pitch_target_ = t.pitch.xenc;
            LOG_INFO << "directed move: heading " << target_heading_ << " (deg)";
        }
        sendMove();
        moving_ = true;
        settled_at_ms_ = 0;
        resetTimer();
        return;
    }

    // Moving: wait for standstill at the target, then hold still a moment before
    // keeping a frame.
    if (settled_at_ms_ == 0) {
        if (!settledAt(t)) return;
        settled_at_ms_ = now_ms_();
        LOG_TRACE_CAT(LogCat::Control)
            << "settled at yaw=" << t.yaw.xenc << " pitch=" << t.pitch.xenc << "; waiting "
            << settle_wait_ms_ << " ms before capture";
        return;
    }

    // The camera free-runs, so this pause is what makes the kept frame one that was
    // exposed with the gimbal at rest (no slew blur) and with the on-camera auto
    // exposure settled on this waypoint's light.
    const long long since_settled = now_ms_() - settled_at_ms_;
    if (since_settled < settle_wait_ms_) return;

    if (camera_.trigger()) {
        if (control_code_ == 0)
            LOG_INFO << "captured waypoint " << (grid_.index() + 1) << "/" << grid_.size();
        else
            LOG_INFO << "captured (directed)";
    } else if (since_settled < settle_wait_ms_ + frame_timeout_ms_) {
        return;  // no fresh frame yet; retry next tick
    } else {
        // Never park on one waypoint: a stalled camera would otherwise stop the
        // whole sweep rather than costing it a single image.
        LOG_WARN << "no frame within " << frame_timeout_ms_ << " ms of settling; skipping "
                 << (control_code_ == 0 ? "waypoint " + std::to_string(grid_.index() + 1) +
                                              "/" + std::to_string(grid_.size())
                                        : std::string("directed capture"));
    }

    moving_ = false;
    settled_at_ms_ = 0;
    if (control_code_ == 0) grid_.advance();
    resetTimer();
}

}  // namespace fgc