#include "fgc/TarisCameraSource.h"

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

#include <algorithm>
#include <atomic>
#include <cctype>
#include <chrono>
#include <cmath>
#include <condition_variable>
#include <cstring>
#include <deque>
#include <clocale>
#include <fstream>
#include <map>
#include <mutex>
#include <sstream>
#include <thread>
#include <vector>

namespace fgc {

using namespace irbgrab;

namespace {

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

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

std::string lower(std::string s) {
    for (auto& c : s) c = static_cast<char>(std::tolower(static_cast<unsigned char>(c)));
    return s;
}

// One line of a sysfs file, or "" if it cannot be read.
std::string sysfs(const std::string& path) {
    std::ifstream f(path);
    std::string   s;
    std::getline(f, s);
    return s;
}

// The SDK changes the process's C locale (seen on tower 111: LC_NUMERIC turned
// to a comma one under LANG=en_US.UTF-8). The program parses and prints numbers
// with the C library, so every SDK call that may do this runs inside one of
// these, which puts the locale back as it was.
class LocaleGuard {
public:
    LocaleGuard() {
        const char* now = std::setlocale(LC_ALL, nullptr);
        saved_ = now ? now : "C";
    }
    ~LocaleGuard() {
        const char* now = std::setlocale(LC_ALL, nullptr);
        if (!now || saved_ != now) std::setlocale(LC_ALL, saved_.c_str());
    }
    LocaleGuard(const LocaleGuard&)            = delete;
    LocaleGuard& operator=(const LocaleGuard&) = delete;

private:
    std::string saved_;
};

constexpr long long kLogEveryMs       = 60000;   // repeated trouble: one line a minute
constexpr long long kTelemetryEveryMs = 5000;
// Diagnostics for pictures that get no thermal frame: a pause in the frame
// stream this long is logged with what the camera said meanwhile, and a summary
// of the stream goes out this often.
constexpr long long kGapLogMs         = 500;
constexpr long long kSummaryEveryMs   = 600000;

}  // namespace

TarisCameraSource::Options TarisCameraSource::fromSpec(const CameraSpec& spec) {
    Options o;
    o.serial           = spec.id;
    o.calib_profile    = spec.calib_profile;
    o.frame_rate_hz    = spec.frame_rate_hz;
    o.max_frame_age_ms = spec.max_frame_age_ms;
    o.library          = spec.library;
    o.interface        = spec.interface;
    o.nuc_interval_ms  = spec.nuc_interval_s * 1000;
    return o;
}

double TarisCameraSource::expectedKelvinPerCount(int calib_profile) {
    switch (calib_profile) {
        case 0:  return 0.01;   // FF (-40-120C)
        case 1:  return 0.02;   // FF (0-600C)
        default: return 0.0;
    }
}

bool TarisCameraSource::processHasNetRaw() {
    std::ifstream f("/proc/self/status");
    std::string   line;
    while (std::getline(f, line)) {
        if (line.rfind("CapEff:", 0) != 0) continue;
        try {
            const unsigned long long caps = std::stoull(line.substr(7), nullptr, 16);
            return (caps >> 13) & 1;   // CAP_NET_RAW
        } catch (const std::exception&) {
            return false;
        }
    }
    return false;
}

struct TarisCameraSource::Impl {
    Options                    o;
    std::shared_ptr<const Api> api;
    std::function<bool()>      has_net_raw;

    // ---- the worker's own (only it calls the SDK) ---------------------------
    Handle      dev        = 0;
    bool        sdk_init   = false;
    bool        connected  = false;
    int         cal        = -1;       // active profile as the camera reports it
    double      kpc        = 0.0;      // measured from the LUT; 0 = not yet
    long long   last_frame_steady = 0;
    double      last_sdk_ts       = -1.0;  // MemInfo time of the last frame taken
    int         scale_tries       = 0;     // frames whose LUT was still empty
    long long   last_telemetry    = 0;
    long long   nuc_or_licence    = 0; // getdata 0x80000017 since the last log line
    long long   nuc_logged_ms     = 0;
    bool        zeros_logged      = false;
    std::string last_offline_logged;

    // Stream diagnostics (see kGapLogMs). Since the last frame taken:
    double      last_cam_ts       = -1.0;  // its MemInfo time, ms
    long long   polls_since_frame = 0;
    std::map<Ret, int> codes_since_frame;  // getdata answers other than a new frame
    // The frame rate (see setFrameRate()): set by restarting the grab once the
    // first frame is in, then checked on the wire.
    int         rate_attempts     = 0;
    bool        rate_pending      = false; // restart the grab with it at the next frame
    long long   wire_check_ms     = 0;     // steady: when to count packets; 0 = none
    long long   wire_packets0     = -1;
    long long   wire_start_ms     = 0;
    size_t      frame_bytes       = 0;
    // NUC run by the program (Options::nuc_interval_ms).
    int         nuc_interval_found = -1;   // the camera's own timer, put back on close
    bool        nuc_ours          = false; // the camera's timer is off and we run it
    long long   last_nuc_ms       = 0;     // steady
    // For the summary:
    long long   sum_frames = 0, sum_gaps = 0, sum_nucs = 0, sum_max_gap = 0;
    double      sum_cam_span      = 0.0;   // camera clock covered by sum_frames
    long long   sum_since_ms      = 0;

    // ---- shared, under m -------------------------------------------------
    std::mutex              m;
    std::condition_variable cv;
    Frame                   latest;
    uint64_t                latest_seq    = 0;
    uint64_t                delivered_seq = 0;
    std::string             why_offline   = "not opened";
    CameraDeviceInfo        info;
    std::deque<long long>   arrivals;      // steady ms of recent frames, for fps
    long long               missed_shots  = 0;
    long long               packets_lost  = 0;
    int                     cal_wanted    = -1;   // a pending setFeature("CalibProfile")
    bool                    cal_done      = false;
    bool                    cal_ok        = false;
    bool                    reset_wanted  = false;
    uint64_t                connects      = 0;
    bool                    nuc_now       = false;  // a NUC of ours is running
    long long               nuc_now_since = 0;      // steady ms it began
    bool                    move_started  = false;  // moving() since the worker last looked
    long long               missed_window = 0;      // misses since the last summary

    std::atomic<bool> running{false};
    std::atomic<bool> gate{false};
    FrameCallback     callback;
    std::thread       worker;

    explicit Impl(Options opts, std::shared_ptr<const Api> a, std::function<bool()> net_raw)
        : o(std::move(opts)), api(std::move(a)), has_net_raw(std::move(net_raw)) {
        if (!has_net_raw) has_net_raw = &TarisCameraSource::processHasNetRaw;
        info.id     = o.serial;
        info.serial = o.serial;
        info.model  = "TarisIR";
    }

    void setOffline(const std::string& why) {
        {
            std::lock_guard<std::mutex> l(m);
            why_offline    = why;
            info.streaming = false;
        }
        if (why != last_offline_logged) {
            LOG_WARN << "thermal camera " << o.serial << " offline: " << why;
            last_offline_logged = why;
        }
    }

    // Sleep up to ms, or less if close() wants the worker to stop or another
    // thread has asked for a profile switch or a reset.
    void pause(long long ms) {
        std::unique_lock<std::mutex> l(m);
        cv.wait_for(l, std::chrono::milliseconds(ms),
                    [this] {
                        return !running.load() || cal_wanted >= 0 || reset_wanted || move_started;
                    });
    }

    // ---- parameter helpers -------------------------------------------------
    bool getInt32(uint32_t what, int32_t& v) {
        return ok(api->dev_getparam(dev, what, &v, kTypeInt32));
    }
    bool getIdxInt32(uint32_t what, int32_t index, int32_t& v) {
        IdxInt32 p{index, 0};
        if (!ok(api->dev_getparam(dev, what, &p, kTypeIdxInt32))) return false;
        v = p.value;
        return true;
    }
    bool getSingle(uint32_t what, float& v) {
        return ok(api->dev_getparam(dev, what, &v, kTypeSingle));
    }
    bool getString(uint32_t what, std::string& out) {
        std::vector<char> buf(kCharBuffer, 0);
        String s{buf.data(), kCharBuffer};
        if (!ok(api->dev_getparam(dev, what, &s, kTypeString))) return false;
        out.assign(buf.data(), strnlen(buf.data(), buf.size()));
        return true;
    }
    bool getIdxString(uint32_t what, int32_t index, std::string& out) {
        std::vector<char> buf(kCharBuffer, 0);
        IdxString s{index, {buf.data(), kCharBuffer}};
        if (!ok(api->dev_getparam(dev, what, &s, kTypeIdxString))) return false;
        out.assign(buf.data(), strnlen(buf.data(), buf.size()));
        return true;
    }
    Ret setInt32(uint32_t what, int32_t v) { return api->dev_setparam(dev, what, &v, kTypeInt32); }

    // Calib_Active reads as a plain or an indexed value (both work on 4.3.44).
    // Right after a reconnect it can fail for a moment, so it is asked a few times.
    int readCal() {
        for (int attempt = 0; attempt < 10; ++attempt) {
            int32_t v = -1;
            if (getInt32(param::kCalibActive, v) && v >= 0) return v;
            if (getIdxInt32(param::kCalibActive, 0, v) && v >= 0) return v;
            std::this_thread::sleep_for(std::chrono::milliseconds(300));
        }
        return -1;
    }

    // Switch the calibration profile and wait for the camera to finish.
    // Calib_Active reports the new profile at once, but the frames keep the old
    // scale until Calib_ChangeInProgress has run 2, 4, 2 and reached 0 (about
    // 3 s on tower 111; at rest, before any switch, it reads 6). Nothing is
    // polled meanwhile, so no frame of the wrong scale is taken.
    bool switchCal(int profile, std::string& why) {
        int32_t count = 0;
        if (getInt32(param::kCalibCount, count) && (profile < 0 || profile >= count)) {
            why = "calibration profile " + std::to_string(profile) + " does not exist (camera has " +
                  std::to_string(count) + ")";
            return false;
        }
        IdxInt32 v{0, profile};
        const Ret r = api->dev_setparam(dev, param::kCalibActive, &v, kTypeIdxInt32);
        if (!ok(r)) {
            why = "cannot set calibration profile " + std::to_string(profile) + ": " + describe(r);
            return false;
        }
        const long long until = steadyMs() + o.calib_timeout_ms;
        int32_t busy = 1;
        while (steadyMs() < until) {
            if (getInt32(param::kCalibChangeInProgress, busy) && busy == 0) break;
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        if (busy != 0) {
            why = "calibration profile change did not finish within " +
                  std::to_string(o.calib_timeout_ms) + " ms";
            return false;
        }
        cal         = readCal();
        kpc         = 0.0;   // measure the new scale from the next frames' LUT
        scale_tries = 0;
        if (cal != profile) {
            why = "camera reports profile " + std::to_string(cal) + " after switching to " +
                  std::to_string(profile);
            return false;
        }
        std::string name;
        getIdxString(param::kCalibName, cal, name);
        LOG_INFO << "thermal camera " << o.serial << ": calibration profile " << cal
                 << (name.empty() ? "" : " (" + name + ")");
        return true;
    }

    // ---- connecting --------------------------------------------------------
    std::string linkHint() const {
        if (o.interface.empty()) return {};
        const std::string base  = "/sys/class/net/" + o.interface;
        const std::string state = sysfs(base + "/operstate");
        if (state.empty()) return "; interface " + o.interface + " does not exist";
        std::string hint = "; " + o.interface + " is " + state;
        const std::string mtu = sysfs(base + "/mtu");
        if (!mtu.empty() && mtu != "1500") hint += ", MTU " + mtu + " (the camera needs 1500)";
        return hint;
    }

    void freeDevice() {
        if (dev) api->dev_free(&dev);
        dev = 0;
    }

    void disconnect() {
        if (connected) {
            api->dev_stopgrab(dev, 0);
            api->dev_disconnect(dev);
        }
        connected = false;
        freeDevice();
    }

    bool connect(std::string& why) {
        // The device type the camera package registered.
        std::vector<char> names(kCharBuffer, 0);
        Ret r = api->dll_devicetypenames(names.data(), kCharBuffer);
        if (!ok(r)) {
            why = "SDK lists no device types: " + describe(r);
            return false;
        }
        int type = -1, i = 0;
        std::stringstream ss(std::string(names.data(), strnlen(names.data(), names.size())));
        for (std::string n; std::getline(ss, n, ';'); ++i)
            if (lower(n).find("taris") != std::string::npos) { type = i; break; }
        if (type < 0) {
            why = "SDK has no TarisIR device type (camera package infratec-itcamtarisirmini "
                  "missing?)";
            return false;
        }

        std::vector<char> ini(kCharBuffer, 0);
        r = api->dev_create(&dev, type, ini.data());
        if (!ok(r)) {
            dev = 0;
            why = "cannot create the device: " + describe(r);
            return false;
        }

        // Search first: it fills the SDK's adapter table that connect() needs.
        int32_t count = 0;
        r = api->dev_search(dev, &count);
        if (!ok(r) || count <= 0) {
            freeDevice();
            why = "no thermal camera found on the network" + linkHint();
            return false;
        }
        std::string              chosen;
        std::vector<std::string> seen;
        for (int32_t k = 0; k < count && k < 32; ++k) {
            std::string s;
            if (!getIdxString(param::kSearchString, k, s)) break;
            seen.push_back(s);
            if (chosen.empty() && s.find(o.serial) != std::string::npos) chosen = s;
        }
        if (chosen.empty()) {
            std::string list;
            for (const auto& s : seen) list += (list.empty() ? "" : ", ") + s;
            freeDevice();
            why = "camera " + o.serial + " not found (found: " + (list.empty() ? "none" : list) + ")";
            return false;
        }

        // SN:<serial>|<MAC>|<IP>#<adapter>. The adapter index moves when Docker or
        // VPN interfaces come and go, so if the one found does not connect, try
        // them all.
        std::vector<std::string> candidates{chosen};
        const std::string base = chosen.substr(0, chosen.rfind('#'));
        for (int a = 0; a < 16; ++a) {
            std::string c = base + "#" + std::to_string(a);
            if (c != chosen) candidates.push_back(c);
        }
        Ret last = 0;
        for (const auto& c : candidates) {
            std::vector<char> buf(kCharBuffer, 0);
            std::memcpy(buf.data(), c.data(), std::min<size_t>(c.size(), kCharBuffer - 1));
            last = api->dev_connect(dev, buf.data());
            if (ok(last)) {
                connected = true;
                LOG_INFO << "thermal camera connected via " << c;
                break;
            }
        }
        if (!connected) {
            freeDevice();
            why = "cannot connect to " + chosen + ": " + describe(last) + linkHint();
            return false;
        }

        // Profile: it sets the counts' scale, so it is checked before any frame.
        cal = readCal();
        if (cal < 0) {
            disconnect();
            why = "cannot read the active calibration profile";
            return false;
        }
        if (cal != o.calib_profile) {
            LOG_INFO << "thermal camera on calibration profile " << cal << ", switching to "
                     << o.calib_profile;
            std::string err;
            if (!switchCal(o.calib_profile, err)) {
                disconnect();
                why = err;
                return false;
            }
        }

        // Polling, before and again after starting: the SDK resets it on startgrab.
        setInt32(param::kPollingEnable, 1);
        r = api->dev_startgrab(dev, 0);
        if (!ok(r)) {
            disconnect();
            why = "cannot start grabbing: " + describe(r);
            return false;
        }
        setInt32(param::kPollingEnable, 1);

        rate_attempts = 0;
        rate_pending  = true;
        wire_check_ms = 0;

        std::string model;
        getString(param::kCameraName, model);
        {
            std::lock_guard<std::mutex> l(m);
            if (!model.empty()) info.model = model;
            ++connects;
        }
        takeOverNuc();

        kpc               = 0.0;
        scale_tries       = 0;
        last_sdk_ts       = -1.0;
        last_cam_ts       = -1.0;
        polls_since_frame = 0;
        codes_since_frame.clear();
        zeros_logged      = false;
        last_frame_steady = steadyMs();   // grace for the first frame
        return true;
    }

    // ---- frames ------------------------------------------------------------
    // Take the newest frame from the SDK, if there is one.
    void poll() {
        ++polls_since_frame;
        // As before every getdata in the vendor's examples: without it the SDK
        // now and then answers 0x80000001 instead of a frame.
        setInt32(param::kPollingEnable, 1);
        Handle mem = 0;
        const Ret r = api->dev_getdata(dev, kMemIrDigitFrame, &mem);
        if (!ok(r)) {
            ++codes_since_frame[r];
            if (r == kErrNucOrLicence) {
                ++nuc_or_licence;
                const long long now = steadyMs();
                if (now - nuc_logged_ms >= kLogEveryMs) {
                    LOG_WARN << "thermal camera: " << describe(r) << " x" << nuc_or_licence
                             << " in the last minute; if this persists, check the licence in "
                                "/var/lib/infratec/.license before the NUC";
                    nuc_logged_ms  = now;
                    nuc_or_licence = 0;
                }
            }
            return;
        }
        struct MemGuard {
            const Api* api;
            Handle*    h;
            ~MemGuard() { if (*h) api->mem_free(h); }
        } guard{api.get(), &mem};

        uint32_t w = 0, h = 0, type = 0;
        Handle   ptr   = 0;
        int32_t  bytes = 0;
        if (!ok(api->mem_getdimension(mem, &w, &h, &type)) ||
            !ok(api->mem_getdataptr(mem, &ptr, &bytes)) || !ptr || w == 0 || h == 0)
            return;
        const size_t need = static_cast<size_t>(w) * h * 2;
        if (bytes < 0 || static_cast<size_t>(bytes) < need) return;
        frame_bytes = need;

        // Polling hands out the newest frame again until a newer one arrives. The
        // camera's frame counter stays 0 (TarisIR mini, SDK 4.3.44), so a new
        // frame is told by its time.
        MemInfo mi{};
        mi.struct_size = sizeof(MemInfo);
        if (!ok(api->mem_getinfo(mem, &mi))) {
            mi.image_num    = -1;
            mi.timestamp_ms = -1.0;
        }
        if (mi.timestamp_ms >= 0.0 && mi.timestamp_ms == last_sdk_ts) {   // seen it
            ++codes_since_frame[kSuccess];
            return;
        }
        last_sdk_ts = mi.timestamp_ms;

        const auto* counts = reinterpret_cast<const uint16_t*>(ptr);
        // The first frames after starting are all zero.
        if (std::all_of(counts, counts + static_cast<size_t>(w) * h,
                        [](uint16_t c) { return c == 0; })) {
            if (!zeros_logged) LOG_INFO << "thermal camera: discarding zero frames after start";
            zeros_logged = true;
            return;
        }

        if (kpc == 0.0) measureScale(mem);

        Frame f;
        f.width        = w;
        f.height       = h;
        f.channels     = 1;
        f.layout       = PixelLayout::Mono16;
        f.timestamp_ms = unixMs();
        f.cam_id       = 0;
        f.data.assign(reinterpret_cast<const uint8_t*>(ptr),
                      reinterpret_cast<const uint8_t*>(ptr) + need);
        f.brightness = sampleBrightness(f.data.data(), f.data.size(), f.layout, w, h);
        ThermalFrameInfo t;
        t.cal              = cal;
        t.kelvin_per_count = kpc > 0.0 ? kpc : expectedKelvinPerCount(cal);
        t.missed_packets   = mi.missed_packets;
        t.image_num        = mi.image_num;
        f.thermal          = t;

        const long long now = steadyMs();
        noteFrame(now, mi.timestamp_ms);
        last_frame_steady   = now;
        {
            std::lock_guard<std::mutex> l(m);
            latest = std::move(f);
            ++latest_seq;
            ++info.frames_delivered;
            packets_lost += std::max(0, mi.missed_packets);
            info.frames_dropped = missed_shots + packets_lost;
            info.width          = w;
            info.height         = h;
            info.payload_bytes  = static_cast<long long>(need);
            info.streaming      = true;
            why_offline.clear();
            arrivals.push_back(now);
            while (!arrivals.empty() && now - arrivals.front() > 10000) arrivals.pop_front();
            if (arrivals.size() > 1)
                info.actual_fps = (arrivals.size() - 1) * 1000.0 /
                                  std::max<long long>(1, arrivals.back() - arrivals.front());
        }
        cv.notify_all();
        last_offline_logged.clear();
    }

    // The counts' scale, from the LUT at emissivity and transmission 1, where it
    // is a straight line through zero. The first frames after connecting come
    // with an empty table, so it is asked again on later ones; frames meanwhile
    // use the documented scale, and if the table stays empty, so does the run.
    static constexpr int kScaleTries = 50;
    void measureScale(Handle mem) {
        std::vector<float> lut(65536, 0.f);
        const Ret r = api->mem_getdvlut(mem, lut.data(), 1.f, 1.f, 293.15f, 293.15f);
        const double expected = expectedKelvinPerCount(cal);
        double       measured = 0.0;
        if (ok(r)) measured = (static_cast<double>(lut[40000]) - lut[30000]) / 10000.0;
        if (!(measured > 0.0) && ++scale_tries < kScaleTries) return;   // try the next frame
        LOG_DEBUG << "thermal camera LUT after " << scale_tries << " empty: " << describe(r)
                  << ", [0] " << lut[0] << " [29315] " << lut[29315] << " [65280] "
                  << lut[65280] << " [65535] " << lut[65535] << " K";
        if (!(measured > 0.0)) {
            kpc = expected > 0.0 ? expected : -1.0;   // -1: unknown, and not asked again
            LOG_WARN << "thermal camera: no usable LUT (" << describe(r) << "); taking the "
                     << "documented " << expected << " K per count for profile " << cal;
        } else {
            kpc = measured;
            if (expected > 0.0 && std::abs(kpc - expected) > expected * 0.01)
                LOG_WARN << "thermal camera: profile " << cal << " has " << kpc
                         << " K per count, not the documented " << expected;
            else
                LOG_INFO << "thermal camera: profile " << cal << ", " << kpc << " K per count";
        }
    }

    // ---- frame rate ----------------------------------------------------------
    // The camera sends 50 fps from power-up: 31 MB/s and 22 000 packets a second
    // for the SDK and the CPU to take in, for one picture every few seconds. Set
    // right after connecting, Framerate_Hz answers ok and changes nothing; set
    // while grabbing, it thins out what the SDK hands over but the camera keeps
    // sending. Set while the grab is stopped, after the stream has run, it holds
    // on the wire (tower 111: 5 Hz, 2 100 packets a second). So that is what is
    // done once the first frame is in, and checked on the port's packet count.
    void setFrameRate() {
        float rate = static_cast<float>(o.frame_rate_hz);
        const Ret r = api->dev_setparam(dev, param::kFramerateHz, &rate, kTypeSingle);
        ++rate_attempts;
        if (!ok(r))
            LOG_WARN << "thermal camera: cannot set " << o.frame_rate_hz << " Hz: " << describe(r)
                     << "; it runs at its own rate";
    }

    static long long rxPackets(const std::string& interface) {
        if (interface.empty()) return -1;
        std::ifstream f("/sys/class/net/" + interface + "/statistics/rx_packets");
        long long v = -1;
        f >> v;
        return f ? v : -1;
    }

    void restartForRate() {
        rate_pending = false;
        api->dev_stopgrab(dev, 0);
        setFrameRate();
        setInt32(param::kPollingEnable, 1);
        const Ret r = api->dev_startgrab(dev, 0);
        setInt32(param::kPollingEnable, 1);
        last_frame_steady = steadyMs();   // grace for the restart
        if (!ok(r)) {
            setOffline("cannot restart grabbing: " + describe(r));
            disconnect();
            return;
        }
        wire_check_ms = steadyMs() + 1000;   // count packets once the stream is up again
        wire_packets0 = -1;
    }

    // Packets per second on the camera's port against what the rate should send.
    void checkWire() {
        if (wire_check_ms == 0 || steadyMs() < wire_check_ms) return;
        if (o.interface.empty()) {
            wire_check_ms = 0;
            LOG_INFO << "thermal camera: " << o.frame_rate_hz
                     << " fps set; not checked on the wire (no [Camera.tir] interface)";
            return;
        }
        const long long now = steadyMs(), pk = rxPackets(o.interface);
        if (pk < 0) { wire_check_ms = 0; return; }
        if (wire_packets0 < 0) {            // start counting
            wire_packets0 = pk;
            wire_start_ms = now;
            wire_check_ms = now + 3000;
            return;
        }
        wire_check_ms = 0;
        const double pps      = (pk - wire_packets0) * 1000.0 / std::max<long long>(1, now - wire_start_ms);
        const double per_fr   = frame_bytes > 0 ? frame_bytes / 1464.0 : 420.0;   // GVSP payload
        const double fps_wire = pps / per_fr;
        if (fps_wire <= o.frame_rate_hz * 1.5) {
            LOG_INFO << "thermal camera: " << std::lround(fps_wire * 10) / 10.0 << " fps on "
                     << o.interface << " (" << std::lround(pps) << " packets/s), set on attempt "
                     << rate_attempts;
        } else if (rate_attempts >= 3) {
            LOG_WARN << "thermal camera: still " << std::lround(fps_wire) << " fps on " << o.interface
                     << " (" << std::lround(pps) << " packets/s), not " << o.frame_rate_hz
                     << "; frame rate not taken";
        } else {
            LOG_INFO << "thermal camera: " << std::lround(fps_wire) << " fps on " << o.interface
                     << ", not " << o.frame_rate_hz << "; restarting the grab again";
            rate_pending = true;
        }
    }

    // ---- NUC ---------------------------------------------------------------
    // Switch the camera's own NUC timer off: from now on the program runs the
    // NUC, while the gimbal moves. The timer's value goes back on close. (Read
    // back right after a set, the camera still reports the old value.)
    void takeOverNuc() {
        nuc_ours = false;
        if (o.nuc_interval_ms <= 0) {
            LOG_INFO << "thermal camera: NUC left to the camera's own timer";
            return;
        }
        int32_t found = -1;
        if (!getInt32(param::kNucInterval, found)) {
            LOG_WARN << "thermal camera: cannot read NUC_Interval; NUC left to the camera";
            return;
        }
        if (nuc_interval_found < 0) nuc_interval_found = found;
        const Ret r = setInt32(param::kNucInterval, 0);
        if (!ok(r)) {
            LOG_WARN << "thermal camera: cannot switch the camera's NUC timer off (" << describe(r)
                     << "); a picture may now and then fall into a NUC";
            return;
        }
        nuc_ours    = true;
        last_nuc_ms = steadyMs() - o.nuc_interval_ms;   // the first move runs one
        LOG_INFO << "thermal camera: NUC every " << o.nuc_interval_ms / 1000
                 << " s at the start of a move (camera's own timer: " << found << " s, now off)";
    }

    // Put the camera's timer back, so it corrects itself without us.
    void handBackNuc() {
        if (!nuc_ours || nuc_interval_found <= 0) return;
        const Ret r = setInt32(param::kNucInterval, nuc_interval_found);
        LOG_INFO << "thermal camera: NUC timer back to " << nuc_interval_found << " s ("
                 << describe(r) << ")";
        nuc_ours = false;
    }

    // A NUC when one is due and the gimbal has just started moving, or when it
    // is overdue (twice the interval) with no move to wait for.
    void serviceNuc() {
        bool moved;
        {
            std::lock_guard<std::mutex> l(m);
            moved        = move_started;
            move_started = false;   // always: pause() wakes on it
        }
        if (!nuc_ours) return;
        const long long since = steadyMs() - last_nuc_ms;
        if (moved ? since < o.nuc_interval_ms : since < 2LL * o.nuc_interval_ms) return;
        runNuc(moved ? "move" : "no move for a while");
    }

    void runNuc(const char* why) {
        const long long start = steadyMs();
        {
            std::lock_guard<std::mutex> l(m);
            nuc_now       = true;
            nuc_now_since = start;
        }
        Int2 nuc{kNucMode1Point, 0};
        // Returns once the NUC is done, about 0.7 s.
        const Ret r = api->dev_setparam(dev, param::kNucPerform, &nuc, kType2Int32);
        const long long done = steadyMs();
        {
            std::lock_guard<std::mutex> l(m);
            nuc_now = false;
            // A frame from before the NUC is no longer fresh for a picture.
            delivered_seq = latest_seq;
        }
        last_nuc_ms = done;
        ++sum_nucs;
        // A frame is due as soon as the NUC is over; do not count it as a stall.
        last_frame_steady = std::max(last_frame_steady, done);
        if (!ok(r)) {
            LOG_WARN << "thermal camera: NUC failed (" << describe(r)
                     << "); handing NUC back to the camera's own timer";
            handBackNuc();
            return;
        }
        LOG_DEBUG << "thermal camera: NUC (" << why << ") took " << done - start << " ms";
    }

    // A new frame: log the pause before it if it was long, and count it.
    void noteFrame(long long now, double cam_ts) {
        const long long host_gap = now - last_frame_steady;
        const double    cam_gap  = last_cam_ts >= 0.0 && cam_ts >= 0.0 ? cam_ts - last_cam_ts : -1.0;
        if (last_cam_ts >= 0.0) {
            ++sum_frames;
            if (cam_gap > 0.0) sum_cam_span += cam_gap;
            sum_max_gap = std::max(sum_max_gap, host_gap);
            // A pause of our own making is no news.
            const bool ours = now - last_nuc_ms < 2000;
            if ((host_gap > kGapLogMs || cam_gap > kGapLogMs) && !ours) {
                ++sum_gaps;
                std::string codes;
                for (const auto& [code, n] : codes_since_frame)
                    codes += (codes.empty() ? "" : ", ") +
                             (code == kSuccess ? std::string("same frame") : describe(code)) +
                             " x" + std::to_string(n);
                LOG_INFO << "thermal camera: frame gap " << host_gap << " ms (camera clock "
                         << static_cast<long long>(cam_gap) << " ms), " << polls_since_frame
                         << " polls: " << (codes.empty() ? "-" : codes);
            }
        }
        last_cam_ts       = cam_ts;
        polls_since_frame = 0;
        codes_since_frame.clear();
        if (now - sum_since_ms >= kSummaryEveryMs) summary(now);
    }

    void summary(long long now) {
        if (sum_since_ms != 0 && sum_frames > 0) {
            long long missed;
            {
                std::lock_guard<std::mutex> l(m);
                missed        = missed_window;
                missed_window = 0;
            }
            const double fps = sum_cam_span > 0.0 ? sum_frames * 1000.0 / sum_cam_span : 0.0;
            LOG_INFO << "thermal camera, last " << (now - sum_since_ms) / 60000 << " min: "
                     << sum_frames << " frames (" << std::lround(fps * 10) / 10.0
                     << " fps by the camera's clock), longest gap " << sum_max_gap << " ms, "
                     << sum_gaps << " unexplained over " << kGapLogMs << " ms, " << sum_nucs
                     << " NUC" << (nuc_ours ? " (ours)" : "") << ", "
                     << missed << " picture(s) without a thermal frame";
        }
        sum_frames = sum_gaps = sum_nucs = sum_max_gap = 0;
        sum_cam_span = 0.0;
        sum_since_ms = now;
    }

    void telemetry() {
        float k = 0.f;
        if (getSingle(param::kCameraTempHousing, k) && k > 0.f) {
            std::lock_guard<std::mutex> l(m);
            info.temperature_c = k - 273.15;
        }
    }

    // ---- the worker ------------------------------------------------------
    void run() {
        if (!api) {
            std::string err;
            LocaleGuard keep;
            api = irbgrab::load(o.library, err);
            if (!api) {
                setOffline(err + " (InfraTec SDK not installed?)");
                return;
            }
        }
        if (!has_net_raw()) {
            setOffline("this binary may not open raw sockets (no cap_net_raw): run deploy.sh, "
                       "which sets it, or sudo setcap cap_net_raw+ep build/fire_gimbal_control");
            return;
        }
        Ret r;
        {
            LocaleGuard keep;
            r = api->dll_init();
        }
        if (!ok(r)) {
            setOffline("SDK init failed: " + describe(r));
            return;
        }
        sdk_init = true;
        std::vector<char> ver(kCharBuffer, 0);
        if (ok(api->dll_version(ver.data(), kCharBuffer)))
            LOG_INFO << "IRBGRAB SDK " << ver.data();

        long long retry = o.retry_min_ms;
        const long long poll_ms =
            std::max<long long>(5, static_cast<long long>(500.0 / o.frame_rate_hz));
        while (running) {
            LocaleGuard keep_locale;   // every SDK call in this pass, polling included
            if (!connected) {
                {
                    // Connecting is what a reset asks for; it must not keep the
                    // retry pause from pausing.
                    std::lock_guard<std::mutex> l(m);
                    reset_wanted = false;
                    move_started = false;   // nor may a move cut the retry pause short
                }
                std::string why;
                bool        up;
                {
                    LocaleGuard keep;
                    up = connect(why);
                }
                if (up) {
                    retry = o.retry_min_ms;
                    std::lock_guard<std::mutex> l(m);
                    why_offline = "waiting for the first frame";
                } else {
                    setOffline(why);
                    pause(retry);
                    retry = std::min<long long>(retry * 2, o.retry_max_ms);
                    continue;
                }
            }
            serviceRequests();
            if (!connected) continue;
            serviceNuc();
            poll();
            if (rate_pending && last_cam_ts >= 0.0) restartForRate();   // once a frame is in
            if (!connected) continue;
            checkWire();

            const long long now = steadyMs();
            if (now - last_frame_steady > o.stall_ms) {
                setOffline("no frame for " + std::to_string(o.stall_ms) + " ms; reconnecting");
                disconnect();
                std::lock_guard<std::mutex> l(m);
                ++info.restarts;
                continue;
            }
            if (now - last_telemetry >= kTelemetryEveryMs) {
                telemetry();
                last_telemetry = now;
            }
            pause(poll_ms);
        }
        LocaleGuard keep;
        if (connected) handBackNuc();
        disconnect();
        if (sdk_init) api->dll_uninit();
        sdk_init = false;
    }

    // A profile switch or a reset asked for from another thread.
    void serviceRequests() {
        int  want  = -1;
        bool reset = false;
        {
            std::lock_guard<std::mutex> l(m);
            want         = cal_wanted;
            reset        = reset_wanted;
            reset_wanted = false;
        }
        if (reset) {
            LOG_INFO << "thermal camera: reset requested, reconnecting";
            disconnect();
            std::lock_guard<std::mutex> l(m);
            ++info.restarts;
            return;
        }
        if (want < 0) return;
        std::string why;
        bool        done = switchCal(want, why);
        if (done) o.calib_profile = want;   // a reconnect keeps it
        else LOG_WARN << "thermal camera: " << why;
        {
            std::lock_guard<std::mutex> l(m);
            cal_wanted = -1;
            cal_done   = true;
            cal_ok     = done;
            // Frames from before the switch are on the old scale: never hand one out.
            delivered_seq = latest_seq;
        }
        cv.notify_all();
    }
};

TarisCameraSource::TarisCameraSource(Options opts, std::shared_ptr<const irbgrab::Api> api,
                                     std::function<bool()> has_net_raw)
    : impl_(std::make_unique<Impl>(std::move(opts), std::move(api), std::move(has_net_raw))) {}

TarisCameraSource::~TarisCameraSource() { close(); }

void TarisCameraSource::open() {
    if (impl_->running.exchange(true)) return;
    {
        std::lock_guard<std::mutex> l(impl_->m);
        impl_->why_offline = "connecting";
    }
    impl_->worker = std::thread([this] { impl_->run(); });
    LOG_INFO << "thermal camera " << impl_->o.serial << ": connecting in the background";
}

void TarisCameraSource::close() {
    if (!impl_->running.exchange(false)) return;
    impl_->cv.notify_all();
    if (impl_->worker.joinable()) impl_->worker.join();
    std::lock_guard<std::mutex> l(impl_->m);
    impl_->why_offline    = "closed";
    impl_->info.streaming = false;
}

void TarisCameraSource::start() { impl_->gate = true; }
void TarisCameraSource::stop() { impl_->gate = false; }

void TarisCameraSource::setFrameCallback(FrameCallback cb) { impl_->callback = std::move(cb); }

void TarisCameraSource::moving() {
    {
        std::lock_guard<std::mutex> l(impl_->m);
        impl_->move_started = true;
    }
    impl_->cv.notify_all();   // the worker may be pausing between polls
}

bool TarisCameraSource::frameReady() {
    Impl&                       d = *impl_;
    std::lock_guard<std::mutex> l(d.m);
    if (!d.running || !d.why_offline.empty()) return true;   // nothing to wait for
    if (d.nuc_now) return false;
    return d.latest_seq > d.delivered_seq &&
           unixMs() - d.latest.timestamp_ms <= d.o.max_frame_age_ms;
}

bool TarisCameraSource::trigger() {
    Impl& d = *impl_;
    if (!d.gate || !d.callback) return false;
    Frame f;
    {
        std::lock_guard<std::mutex> l(d.m);
        const bool new_frame = d.latest_seq > d.delivered_seq;
        const long long age  = d.latest_seq ? unixMs() - d.latest.timestamp_ms : -1;
        const bool fresh     = new_frame && age <= d.o.max_frame_age_ms;
        if (!fresh) {
            // Every miss, with what tells its cause apart: the stream paused (and
            // whether a NUC was running), or nothing came since the last picture.
            ++d.missed_shots;
            ++d.missed_window;
            d.info.frames_dropped = d.missed_shots + d.packets_lost;
            const long long now = steadyMs();
            std::string why = !d.why_offline.empty()
                                  ? d.why_offline
                                  : "newest frame " + std::to_string(age) + " ms old (limit " +
                                        std::to_string(d.o.max_frame_age_ms) + " ms)" +
                                        (new_frame ? "" : ", none since the last picture") +
                                        (d.nuc_now ? ", NUC in progress for " +
                                                         std::to_string(now - d.nuc_now_since) + " ms"
                                                   : "");
            LOG_WARN << "thermal camera: no thermal frame for this picture: " << why;
            return false;
        }
        f               = d.latest;
        d.delivered_seq = d.latest_seq;
    }
    d.callback(f);
    return true;
}

std::vector<CameraDeviceInfo> TarisCameraSource::deviceInfo() {
    std::lock_guard<std::mutex> l(impl_->m);
    return {impl_->info};
}

std::string TarisCameraSource::offline(int camera_index) {
    if (camera_index != 0) return "no such camera";
    std::lock_guard<std::mutex> l(impl_->m);
    return impl_->why_offline;
}

bool TarisCameraSource::grabFrame(int camera_index, Frame& out, int timeout_ms) {
    if (camera_index != 0) return false;
    Impl&                        d = *impl_;
    std::unique_lock<std::mutex> l(d.m);
    const uint64_t               after = d.latest_seq;
    if (!d.cv.wait_for(l, std::chrono::milliseconds(timeout_ms),
                       [&] { return d.latest_seq > after || !d.running; }) ||
        d.latest_seq <= after)
        return false;
    out = d.latest;
    return true;
}

bool TarisCameraSource::readFeature(int camera_index, const std::string& name, double& value) {
    if (camera_index != 0) return false;
    std::lock_guard<std::mutex> l(impl_->m);
    if (name == "CalibProfile") {
        if (!impl_->latest.thermal) return false;
        value = impl_->latest.thermal->cal;
        return true;
    }
    if (name == "KelvinPerCount") {
        if (!impl_->latest.thermal) return false;
        value = impl_->latest.thermal->kelvin_per_count;
        return true;
    }
    if (name == "HousingTemperature") {
        if (impl_->info.temperature_c == 0.0) return false;
        value = impl_->info.temperature_c;
        return true;
    }
    return false;
}

bool TarisCameraSource::setFeature(int camera_index, const std::string& name,
                                   const std::string& value) {
    if (camera_index != 0 || name != "CalibProfile") return false;
    int profile = -1;
    try {
        size_t used = 0;
        profile     = std::stoi(value, &used);
        if (used != value.size()) return false;
    } catch (const std::exception&) {
        return false;
    }
    Impl&                        d = *impl_;
    std::unique_lock<std::mutex> l(d.m);
    if (!d.running || !d.why_offline.empty()) return false;
    d.cal_wanted = profile;
    d.cal_done   = false;
    d.cv.notify_all();
    if (!d.cv.wait_for(l, std::chrono::milliseconds(d.o.calib_timeout_ms + 5000),
                       [&] { return d.cal_done || !d.running; }))
        return false;
    return d.cal_done && d.cal_ok;
}

bool TarisCameraSource::resetCamera(int camera_index) {
    if (camera_index != 0) return false;
    Impl&                        d = *impl_;
    std::unique_lock<std::mutex> l(d.m);
    if (!d.running) return false;
    const uint64_t before = d.connects;
    const uint64_t seq    = d.latest_seq;
    d.reset_wanted = true;
    d.cv.notify_all();
    // Back once it has connected again and delivered a frame.
    return d.cv.wait_for(l, std::chrono::seconds(45), [&] {
        return (d.connects > before && d.latest_seq > seq) || !d.running;
    }) && d.running;
}

}  // namespace fgc