#include "fgc/CaptureScheduler.h"

#include "fgc/CommandParser.h"
#include "fgc/Logger.h"

#include <chrono>
#include <cstdint>
#include <cstdlib>
#include <optional>

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,
                                   double image_rate, Geometry geometry, ScanGrid& grid,
                                   std::function<long long()> now_ms)
    : motor_(motor),
      camera_(camera),
      geometry_(geometry),
      grid_(grid),
      now_ms_(now_ms ? std::move(now_ms) : steadyNowMs),
      image_rate_(image_rate) {
    timer_start_ = now_ms_();
}

void CaptureScheduler::setMode(Mode mode, const PanSpec& pan) {
    mode_ = mode;
    pan_  = pan;
    pan_headings_ = mode == Mode::Pan ? panHeadings(pan, pan_step_deg_) : std::vector<double>{};
    pan_index_    = 0;
    pan_unreachable_warned_ = false;
    moving_ = false;  // restart the move/settle cycle cleanly
    settled_at_ms_ = 0;
    rule_started_  = false;
    // 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;
    // Continuous capture is a manual-mode activity: any mode change ends it, so
    // a later return to manual never resumes shooting nobody asked for.
    continuous_ = false;
    continuous_settled_ms_ = 0;
    resetTimer();
}

void CaptureScheduler::setContinuous(bool on) {
    continuous_ = on;
    continuous_settled_ms_ = 0;
    // The first frame comes one interval after switching on, as the first scan
    // waypoint does after `mode auto`.
    resetTimer();
}

void CaptureScheduler::setSettleRule(bool on, const SettleRule::Params& params) {
    use_rule_ = on;
    rule_.setParams(params);
    rule_started_ = false;
}

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.
    motor_.sendCommand(moveCommand(yaw_target_, pitch_present_ ? std::optional<long>(pitch_target_)
                                                               : std::nullopt));
}

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;
}

bool CaptureScheduler::roomToSave() {
    const bool room = !can_save_ || can_save_();
    if (!room && !holding_) {
        holding_ = true;
        LOG_WARN << "saving is behind: holding the scan here until it catches up";
    } else if (room && holding_) {
        holding_ = false;
        LOG_INFO << "saving caught up: the scan carries on";
    }
    return room;
}

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

// Wait for rest, settle, then shoot - the tail of a scan waypoint without the
// move. A snapshot and a continuous-capture frame both come through here, so
// either is exactly what a scan waypoint produces: same file, same CamEvent.
//
// Standstill only, no target check: the operator aims, so there is no target.
// The frame budget is measured from settling, not from the request - a request
// made mid-slew must not spend the camera's budget on the slew.
CaptureScheduler::Shot CaptureScheduler::shootAtRest(const MotorTelemetry& t,
                                                     long long& settled_ms) {
    const bool standstill = t.yaw.standstill && (!pitch_present_ || t.pitch.standstill);
    if (!standstill) {
        settled_ms = 0;  // still slewing (e.g. a jog is in flight)
        return Shot::Waiting;
    }
    if (settled_ms == 0) {
        settled_ms = now_ms_();
        return Shot::Waiting;
    }
    const long long since_settled = now_ms_() - settled_ms;
    if (since_settled < settle_wait_ms_) return Shot::Waiting;
    if (camera_.trigger()) return Shot::Captured;
    if (since_settled > settle_wait_ms_ + frame_timeout_ms_) return Shot::TimedOut;
    return Shot::Waiting;  // no fresh frame yet; retry next tick
}

void CaptureScheduler::serviceSnapshot(const MotorTelemetry& t) {
    switch (shootAtRest(t, snapshot_settled_ms_)) {
        case Shot::Waiting:
            return;
        case Shot::Captured:
            LOG_INFO << "snapshot captured";
            break;
        case Shot::TimedOut:
            LOG_WARN << "snapshot: no frame within " << frame_timeout_ms_
                     << " ms of settling; send 'snapshot' (Space) again";
            break;
    }
    snapshot_pending_ = false;
}

// The scan's cycle without the move: wait the interval, then shoot at rest.
// The timer restarts after each frame kept or given up, exactly as a scan's
// does, so the cadence matches what a scan achieves rather than the bare
// interval.
void CaptureScheduler::serviceContinuous(const MotorTelemetry& t) {
    const double interval_ms = image_rate_ > 0.0 ? 1000.0 / image_rate_ : 0.0;
    if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;
    if (continuous_settled_ms_ == 0 && !roomToSave()) return;  // never mid-settle
    switch (shootAtRest(t, continuous_settled_ms_)) {
        case Shot::Waiting:
            return;
        case Shot::Captured:
            LOG_INFO << "continuous capture: captured";
            break;
        case Shot::TimedOut:
            LOG_WARN << "continuous capture: no frame within " << frame_timeout_ms_
                     << " ms of settling; trying again next interval";
            break;
    }
    continuous_settled_ms_ = 0;
    resetTimer();
}

void CaptureScheduler::tick() {
    // 1. Telemetry.
    MotorTelemetry t = motor_.telemetry();
    pitch_present_ = t.pitch_present;

    // 2. Manual: a snapshot, else continuous capture. Only in manual, so a
    //    request can never inject a frame into a running sweep or fight it for
    //    the settle window. A snapshot goes first; continuous capture resumes
    //    on its own clock afterwards.
    if (mode_ == Mode::Manual) {
        if (snapshot_pending_) serviceSnapshot(t);
        else if (continuous_) serviceContinuous(t);
        return;
    }

    // 3. Capture cycle: the next scan waypoint (auto) or pan position (pan).
    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, and only while
        // there is room to save what it will capture.
        if (interval_ms <= 0.0 || elapsedMs() <= interval_ms) return;
        if (!roomToSave()) return;

        if (mode_ == Mode::Auto) {
            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 {  // pan: the next position, left to right, at the pan's pitch
            if (pan_headings_.empty()) return;
            // Skip positions outside the reachable arc, as the sweep does; try
            // each at most once per tick so a pan with none reachable just idles.
            std::optional<long> yc;
            for (size_t tries = 0; tries < pan_headings_.size() && !yc; ++tries) {
                yc = geometry_.yaw.headingToCounts(pan_headings_[pan_index_]);
                if (yc) break;
                if (!pan_unreachable_warned_) {
                    pan_unreachable_warned_ = true;
                    LOG_WARN << "pan position " << (pan_index_ + 1) << "/" << pan_headings_.size()
                             << " heading " << pan_headings_[pan_index_]
                             << " deg is outside the reachable yaw arc; skipping it (further "
                                "unreachable positions are skipped silently)";
                }
                pan_index_ = (pan_index_ + 1) % pan_headings_.size();
            }
            if (!yc) return;
            yaw_target_   = *yc;
            pitch_target_ = geometry_.pitch.toCounts(pan_.pitch);
            LOG_INFO << "pan move " << (pan_index_ + 1) << "/" << pan_headings_.size()
                     << ": yaw " << pan_headings_[pan_index_] << " pitch " << pan_.pitch
                     << " (deg)";
        }
        sendMove();
        moving_ = true;
        settled_at_ms_ = 0;
        rule_started_  = false;
        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;

    // The keep rule: from here on, watch the frames the camera delivers until
    // brightness says the image has settled, or the frame budget runs out.
    if (use_rule_) {
        if (!rule_started_) {
            rule_started_ = true;
            rule_.reset();
            rule_seq_ = camera_.frameSeq();
            // No frame data from this camera: keep the fixed wait's behaviour.
            if (rule_seq_ == 0) rule_seq_ = UINT64_MAX;
        }
        if (rule_seq_ != UINT64_MAX) {
            SettleRule::Verdict v = SettleRule::Verdict::Waiting;
            for (const auto& f : camera_.framesSince(rule_seq_)) {
                rule_seq_ = f.seq;
                v         = rule_.add(f);
                if (v != SettleRule::Verdict::Waiting) break;
            }
            if (v == SettleRule::Verdict::Waiting) {
                if (since_settled < settle_wait_ms_ + frame_timeout_ms_) return;  // keep watching
                LOG_WARN << "the image did not settle within " << frame_timeout_ms_
                         << " ms of standstill; keeping the newest frame";
            } else {
                LOG_TRACE_CAT(LogCat::Control)
                    << "image settled after " << since_settled << " ms"
                    << (v == SettleRule::Verdict::AtLimit ? " (exposure and gain at their limit)"
                                                          : "");
            }
            rule_seq_ = UINT64_MAX;  // decided: from here on, only the trigger retries
        }
    }

    if (camera_.trigger()) {
        if (mode_ == Mode::Auto)
            LOG_INFO << "captured waypoint " << (grid_.index() + 1) << "/" << grid_.size();
        else
            LOG_INFO << "captured pan position " << (pan_index_ + 1) << "/"
                     << pan_headings_.size();
    } 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 "
                 << (mode_ == Mode::Auto ? "waypoint " : "pan position ")
                 << (mode_ == Mode::Auto ? grid_.index() + 1 : pan_index_ + 1) << "/"
                 << (mode_ == Mode::Auto ? grid_.size() : pan_headings_.size());
    }

    moving_ = false;
    settled_at_ms_ = 0;
    rule_started_  = false;
    if (mode_ == Mode::Auto) grid_.advance();
    else if (!pan_headings_.empty()) pan_index_ = (pan_index_ + 1) % pan_headings_.size();
    resetTimer();
}

}  // namespace fgc