#include "fgc/VimbaCameraSource.h"

// The SDK comes first on purpose. <VmbCPP/VmbCPP.h> pulls in
// VmbCPP/LoggerDefines.h, which on some SDK versions (the 2024-era /opt/VimbaX on
// the LattePanda, though not 2025-3) defines a FUNCTION-like LOG_ERROR(txt,
// errCode). Ours is object-like - `LOG_ERROR << "..."` - so whichever header is
// included last wins the redefinition, and when theirs wins the stream form never
// expands and fails to compile. Take the SDK's macros, drop them, then define
// ours over the top. The #undefs are harmless no-ops on SDKs that lack them.
#include <VmbCPP/VmbCPP.h>
#undef LOG_ERROR
#undef LOG_FREE_TEXT

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

#include <algorithm>
#include <atomic>
#include <chrono>
#include <deque>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>

using namespace VmbCPP;

namespace fgc {

namespace {

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

// Cumulative per-camera acquisition stats. Lives in Impl (NOT the observer) so it
// survives the restart-on-stall, which recreates the observer each session.
struct CamStats {
    // The last frames' brightness, exposure and gain, for the keep rule.
    std::mutex             recent_mutex;
    std::deque<FrameStats> recent;
    uint64_t               seq = 0;
    void pushRecent(const Frame& f) {
        std::lock_guard<std::mutex> lock(recent_mutex);
        recent.push_back(FrameStats{++seq, f.brightness.valid() ? f.brightness.mean : -1.0,
                                    f.exposure_us, f.gain_db});
        while (recent.size() > 64) recent.pop_front();
    }

    std::atomic<long long> delivered{0};
    std::atomic<long long> dropped{0};
    std::atomic<long long> restarts{0};
    std::atomic<unsigned>  last_w{0};
    std::atomic<unsigned>  last_h{0};
    std::atomic<long long> last_bytes{0};
};

// The program's name for a delivered pixel format; Unknown for anything else.
PixelLayout layoutOf(VmbPixelFormatType f) {
    switch (f) {
        case VmbPixelFormatMono8:      return PixelLayout::Mono8;
        case VmbPixelFormatRgb8:       return PixelLayout::RGB8;
        case VmbPixelFormatBayerRG8:   return PixelLayout::BayerRG8;
        case VmbPixelFormatBayerRG12p: return PixelLayout::BayerRG12p;
        default:                       return PixelLayout::Unknown;
    }
}

// Frame observer for the free-running stream: keeps the most recent COMPLETE
// frame. trigger() (once per scan waypoint) reads it out; the intermediate frames
// just give the on-camera auto-exposure/gain time to converge to the current
// light.
class FrameObserver : public IFrameObserver {
public:
    FrameObserver(CameraPtr camera, int cam_id, CamStats* stats)
        : IFrameObserver(camera), cam_id_(cam_id), stats_(stats) {}

    // Copy out the latest complete frame; false if none has arrived yet.
    bool takeLatest(Frame& out) {
        std::lock_guard<std::mutex> lock(mutex_);
        if (!has_) return false;
        out = latest_;
        return true;
    }

    // Timestamp of the latest complete frame (0 if none) - lets the source detect a
    // stalled stream (timestamp stops advancing) without copying the whole frame.
    long long latestTimestamp() {
        std::lock_guard<std::mutex> lock(mutex_);
        return has_ ? latest_.timestamp_ms : 0;
    }

    void FrameReceived(const FramePtr pFrame) override {
        VmbFrameStatusType status;
        VmbErrorType       rc = pFrame->GetReceiveStatus(status);
        if (rc == VmbErrorSuccess && status == VmbFrameStatusComplete) {
            VmbUint32_t       w = 0, h = 0, sz = 0;
            const VmbUchar_t* buf = nullptr;
            if (pFrame->GetWidth(w) == VmbErrorSuccess &&
                pFrame->GetHeight(h) == VmbErrorSuccess &&
                pFrame->GetBufferSize(sz) == VmbErrorSuccess &&
                pFrame->GetBuffer(buf) == VmbErrorSuccess && buf && w && h) {
                Frame f;
                f.width        = w;
                f.height       = h;
                f.cam_id       = cam_id_;
                f.timestamp_ms = nowMs();
                VmbPixelFormatType pf = VmbPixelFormatLast;
                if (pFrame->GetPixelFormat(pf) == VmbErrorSuccess) f.layout = layoutOf(pf);
                size_t bytes = sz;
                if (f.layout != PixelLayout::Unknown) {
                    // The buffer can carry chunk data after the image; keep the image.
                    bytes      = std::min<size_t>(sz, layoutFrameBytes(f.layout, w, h));
                    f.channels = f.layout == PixelLayout::RGB8 ? 3 : 1;
                } else {
                    // A format the program does not handle: say what it holds, as
                    // before layouts existed, and let the pipeline refuse it.
                    f.channels = static_cast<int>(sz / (static_cast<size_t>(w) * h));
                    if (!warned_layout_) {
                        warned_layout_ = true;
                        LOG_WARN << "camera cam" << cam_id_ << ": pixel format 0x" << std::hex
                                 << pf << std::dec << " is not one the program handles "
                                 << "(Mono8, RGB8, BayerRG8, BayerRG12p)";
                    }
                }
                f.data.assign(buf, buf + bytes);
                readChunks(pFrame, f);
                f.brightness = sampleBrightness(f.data.data(), f.data.size(), f.layout, w, h);
                if (stats_) stats_->pushRecent(f);
                {
                    std::lock_guard<std::mutex> lock(mutex_);
                    latest_ = std::move(f);
                    has_    = true;
                }
                if (stats_) {
                    stats_->delivered.fetch_add(1);
                    stats_->last_w.store(w);
                    stats_->last_h.store(h);
                    stats_->last_bytes.store(sz);
                }
                LOG_TRACE_CAT(LogCat::Camera)
                    << "RX frame cam" << cam_id_ << ' ' << w << 'x' << h << ' ' << sz << 'B';
            }
        } else {
            // Incomplete/invalid delivery - usually USB bandwidth starvation on large
            // frames. Surface it instead of silently dropping.
            if (stats_) stats_->dropped.fetch_add(1);
            LOG_WARN << "camera cam" << cam_id_ << " dropped frame (status="
                     << (rc == VmbErrorSuccess ? static_cast<int>(status) : -1) << ')';
        }
        m_pCamera->QueueFrame(pFrame);
    }

private:
    // Exposure, gain and white balance as the camera reports them for this
    // frame. Only when the profile turns chunk mode on; after the first frame
    // without chunk data, stop asking for the rest of the session.
    void readChunks(const FramePtr& frame, Frame& f) {
        if (!chunks_available_) return;
        const VmbErrorType rc = frame->AccessChunkData([&f](ChunkFeatureContainerPtr& c) {
            auto num = [&c](const char* name, double& out) {
                FeaturePtr feat;
                double     v = 0;
                if (c->GetFeatureByName(name, feat) == VmbErrorSuccess &&
                    feat->GetValue(v) == VmbErrorSuccess)
                    out = v;
            };
            num("ChunkExposureTime", f.exposure_us);
            num("ChunkGain", f.gain_db);
            num("ChunkBalanceRatioRed", f.wb_red);
            num("ChunkBalanceRatioBlue", f.wb_blue);
            return VmbErrorSuccess;
        });
        if (rc != VmbErrorSuccess) chunks_available_ = false;
    }

    int        cam_id_;
    CamStats*  stats_;
    bool       warned_layout_    = false;
    bool       chunks_available_ = true;
    std::mutex mutex_;
    Frame      latest_;
    bool       has_ = false;
};

// Read a float/string feature, returning `fallback`/"" if absent (tolerant, for telemetry).
double getFloat(const CameraPtr& cam, const char* name, double fallback = 0.0) {
    FeaturePtr f;
    double     v = fallback;
    if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess && f->GetValue(v) == VmbErrorSuccess)
        return v;
    return fallback;
}
std::string getString(const CameraPtr& cam, const char* name) {
    FeaturePtr  f;
    std::string v;
    if (SP_ACCESS(cam)->GetFeatureByName(name, f) == VmbErrorSuccess && f->GetValue(v) == VmbErrorSuccess)
        return v;
    return {};
}

// Apply a profile by handing its settings XML to the SDK.
//
// This replaces a hand-rolled feature applier, and the reason is validity rather
// than brevity: GenICam writability is stateful (AcquisitionFrameRate is only
// writable once AcquisitionFrameRateEnable is set, Width's maximum depends on
// binning, and so on), so a text list applied line by line can be individually
// plausible and collectively wrong. The XML comes out of the VimbaX Viewer,
// which greys out invalid combinations as the operator configures, and
// LoadSettings replays exactly that. Nothing else may write settings.
bool applyProfile(const CameraPtr& cam, const CameraProfile& profile) {
    VmbFeaturePersistSettings settings{};
    settings.persistType   = VmbFeaturePersistNoLUT;
    // RemoteDevice only: the transport-layer and stream sections of the export
    // describe the host that produced it, not the sensor.
    settings.modulePersistFlags = VmbModulePersistFlagsRemoteDevice;
    settings.maxIterations = 10;   // features can need several passes to settle
    settings.loggingLevel  = VmbLogLevelWarn;

    const VmbErrorType rc = SP_ACCESS(cam)->LoadSettings(profile.xml_path.c_str(), &settings);
    if (rc != VmbErrorSuccess) {
        LOG_ERROR << "camera: applying profile '" << profile.name << "' from "
                  << profile.xml_path << " failed (error " << rc << ')';
        return false;
    }
    LOG_INFO << "camera profile '" << profile.name << "' applied from " << profile.xml_path
             << " (" << profile.settings.entries.size() << " settings)";
    return true;
}

}  // namespace

struct VimbaCameraSource::Impl {
    Impl(std::vector<CameraSpec> s, std::vector<CameraProfile> p)
        : specs(std::move(s)), profiles(std::move(p)), sys(VmbSystem::GetInstance()) {}

    struct Identity { std::string id, model, serial; };
    struct Telemetry {
        double exposure_us = 0, gain_db = 0, temperature_c = 0, actual_fps = 0;
    };

    // One spec and one profile per camera, index-aligned and in open order, so a
    // camera's settings are never those of the camera next to it.
    std::vector<CameraSpec>                specs;
    std::vector<CameraProfile>             profiles;
    VmbSystem&                             sys;
    std::vector<CameraPtr>                 cameras;
    std::vector<IFrameObserverPtr>         observers;
    std::vector<std::unique_ptr<CamStats>> stats;      // one per camera; survives restart
    std::vector<Identity>                  identity;    // read once at open()
    std::vector<Telemetry>                 telemetry;   // live values, cached ~1 Hz
    long long                              telemetry_ms = 0;  // last telemetry refresh
    ICameraSource::FrameCallback           callback;
    bool                                   started          = false;
    long long                              last_captured_ts = 0;  // freshness watermark
    long long                              last_no_frame_warn_ms = 0;
    long long                              session_start_ms = 0;  // this acquisition session
    int                                    failed_restarts  = 0;  // since the last frame arrived
    // Per camera: the value each feature held before `test camera` first changed
    // it, in the order changed - so it can be put back live, without stopping
    // the stream (which is what a profile reload has to do).
    std::vector<std::vector<std::pair<std::string, std::string>>> touched;

    // Begin/end a free-run acquisition session (fresh observers each time). Split out so
    // trigger() can restart the stream when it stalls. Stats persist across restarts.
    void startAcquisition() {
        // Stop first, unconditionally. Clearing `observers` drops the last
        // references to the FrameObserver objects, and until
        // StopContinuousImageAcquisition() has returned the SDK's acquisition
        // thread may still be calling FrameReceived on them - destroying them
        // under it is a use-after-free, not a leak.
        stopAcquisition();
        // A fresh session has delivered nothing, so a watermark from the previous
        // one would make every genuinely-new frame look stale to trigger().
        last_captured_ts = 0;
        session_start_ms = nowMs();
        int cam_id = 0;
        for (auto& cam : cameras) {
            IFrameObserverPtr observer(new FrameObserver(cam, cam_id, stats[cam_id].get()));
            if (cam->StartContinuousImageAcquisition(5, observer) != VmbErrorSuccess)
                throw std::runtime_error("Could not start acquisition");
            observers.push_back(observer);
            ++cam_id;
        }
    }
    void stopAcquisition() {
        for (auto& cam : cameras) cam->StopContinuousImageAcquisition();
        observers.clear();
    }
    // Longest exposure any camera is set to, for how long a frame may take.
    double longestExposureUs() {
        double e = 0;
        for (auto& cam : cameras) e = std::max(e, getFloat(cam, "ExposureTime"));
        return e;
    }

    // Reboot one camera, reopen it by id and re-apply its profile. See
    // CameraStall.h for why this exists.
    bool resetOne(size_t idx) {
        const std::string& id = specs[idx].id;
        LOG_WARN << "camera '" << specs[idx].name << "': resetting it (DeviceReset)";
        const bool was_started = started;
        stopAcquisition();
        FeaturePtr f;
        if (SP_ACCESS(cameras[idx])->GetFeatureByName("DeviceReset", f) != VmbErrorSuccess ||
            f->RunCommand() != VmbErrorSuccess) {
            LOG_ERROR << "camera '" << specs[idx].name << "': DeviceReset is not available";
            if (was_started) startAcquisition();
            return false;
        }
        cameras[idx]->Close();
        // It drops off the bus and re-enumerates: ~10 s for an Alvium 1800 U on
        // tower 111. Reopen it by id as soon as it answers.
        std::this_thread::sleep_for(std::chrono::seconds(3));
        CameraPtr fresh;
        const long long deadline = nowMs() + 45000;
        bool opened = false;
        while (!opened && nowMs() < deadline) {
            CameraPtr c;
            if (sys.GetCameraByID(id.c_str(), c) == VmbErrorSuccess &&
                c->Open(VmbAccessModeFull) == VmbErrorSuccess) {
                fresh  = c;
                opened = true;
            } else {
                std::this_thread::sleep_for(std::chrono::seconds(1));
            }
        }
        if (!opened) {
            LOG_ERROR << "camera '" << specs[idx].name << "' did not come back within 45 s of "
                         "a DeviceReset - it needs a power cycle";
            return false;
        }
        cameras[idx] = fresh;
        if (idx < touched.size()) touched[idx].clear();
        applyProfile(fresh, profiles[idx]);
        if (was_started) startAcquisition();
        LOG_INFO << "camera '" << specs[idx].name << "' is back after a reset";
        return true;
    }

    // Newest frame timestamp across all cameras (0 if none delivered yet).
    long long newestTs() {
        long long t = 0;
        for (auto& obs : observers)
            t = std::max(t, SP_DYN_CAST<FrameObserver>(obs)->latestTimestamp());
        return t;
    }

};

VimbaCameraSource::VimbaCameraSource(std::vector<CameraSpec> specs,
                                     std::vector<CameraProfile> profiles)
    : impl_(std::make_unique<Impl>(std::move(specs), std::move(profiles))) {}

VimbaCameraSource::~VimbaCameraSource() {
    try {
        stop();
        close();
    } catch (const std::exception& e) {
        LOG_WARN << "VimbaCameraSource shutdown: " << e.what();
    }
}

void VimbaCameraSource::open() {
    if (impl_->sys.Startup() != VmbErrorSuccess)
        throw std::runtime_error("Could not start Vimba X API");
    for (const auto& spec : impl_->specs) {
        const std::string& id = spec.id;
        CameraPtr cam;
        if (impl_->sys.GetCameraByID(id.c_str(), cam) != VmbErrorSuccess)
            throw std::runtime_error("Camera '" + spec.name + "' not found: " + id);
        if (const VmbErrorType rc = cam->Open(VmbAccessModeFull); rc != VmbErrorSuccess) {
            std::string why;
            if (rc == VmbErrorInvalidAccess || rc == VmbErrorBusy)
                why = " - another process has it open (is the VimbaX Viewer still running?)";
            else if (rc == VmbErrorNotFound)
                why = " - it disappeared between discovery and open (USB reset?)";
            throw std::runtime_error("Could not open camera '" + spec.name + "': " + id +
                                     " (Vimba error " + std::to_string(rc) + ")" + why);
        }
        impl_->cameras.push_back(cam);
        // Per-camera stats (persist across restart) + identity (read once here).
        impl_->stats.push_back(std::make_unique<CamStats>());
        impl_->telemetry.emplace_back();
        impl_->touched.emplace_back();
        impl_->identity.push_back({id, getString(cam, "DeviceModelName"),
                                   getString(cam, "DeviceSerialNumber")});
        LOG_INFO << "Opened camera '" << spec.name << "' " << id << " ("
                 << impl_->identity.back().model << ')';
    }
    // Configure each camera ONCE, here. Settings are GenICam features that live on
    // the device and survive a stop/start of acquisition, so re-applying them
    // whenever the stream starts is both wasted work (thousands of features) and
    // wrong in principle: it lets a request to *stream* alter the *configuration*.
    // A scan frame and a manual snapshot must come off an identically configured
    // camera, and the only way to guarantee that is to never touch the
    // configuration in between.
    for (size_t i = 0; i < impl_->cameras.size() && i < impl_->profiles.size(); ++i)
        applyProfile(impl_->cameras[i], impl_->profiles[i]);
}

void VimbaCameraSource::close() {
    for (auto& cam : impl_->cameras) cam->Close();
    impl_->cameras.clear();
    impl_->sys.Shutdown();
}

// Start/stop the free-running stream. Configuration is NOT touched here - that
// happened at open(), and changes only through setProfile().
void VimbaCameraSource::start() {
    // Idempotent. Several callers reach here without knowing whether another
    // already has - `start` (scan), `snapshot`, and a [Startup] mode = auto -
    // and re-entering would restart acquisition under the SDK's feet.
    if (impl_->started) return;
    impl_->startAcquisition();
    impl_->started = true;
}

bool VimbaCameraSource::setProfile(int camera_index, const CameraProfile& profile) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->cameras.size() || idx >= impl_->profiles.size()) {
        LOG_WARN << "camera: setProfile for unknown camera index " << camera_index;
        return false;
    }

    impl_->profiles[idx] = profile;
    // A profile sets every feature, so nothing is left to put back by hand.
    if (idx < impl_->touched.size()) impl_->touched[idx].clear();
    if (!impl_->started) {
        // Idle: every feature is writable, so apply directly. start() no longer
        // applies profiles, so skipping this would leave the camera on the old one.
        return applyProfile(impl_->cameras[idx], profile);
    }

    // Binning, ROI and pixel format cannot be written while the stream runs, and a
    // profile may change any of them, so always cycle the stream. Profile switches
    // are rare; a per-feature "is this writable live" allow-list would be fragile.
    try {
        impl_->stopAcquisition();
        const bool ok = applyProfile(impl_->cameras[idx], profile);
        impl_->startAcquisition();
        if (!ok) return false;
    } catch (const std::exception& e) {
        LOG_ERROR << "camera: applying profile '" << profile.name << "' failed: " << e.what();
        return false;
    }
    // The next trigger() must not hand back a frame exposed under the old profile.
    impl_->last_captured_ts = nowMs();
    return true;
}

void VimbaCameraSource::stop() {
    if (!impl_->started) return;
    impl_->stopAcquisition();
    impl_->started = false;
}

bool VimbaCameraSource::trigger() {
    // Deliver the freshest complete frame per waypoint. This host's USB3 stream
    // reliably delivers the first frames of a session but stalls after a burst; if
    // no frame newer than the last capture is available, the stream has stalled -
    // restart acquisition to unstick it and wait briefly for a fresh frame. Without
    // this, keep-latest would hand the same stale frame to every remaining waypoint
    // (all overwriting one file).
    const long long newest = impl_->newestTs();

    // A session that has delivered NOTHING is warming up, not stalled. These are
    // different states that both fail the "no newer frame" test, and conflating
    // them is expensive: restarting throws away the warm-up and blocks the control
    // thread for 3 s, so a cold stream would be repeatedly reset just as it was
    // about to produce its first frame. Return empty-handed instead; the caller
    // retries every tick.
    //
    // But not forever. A session that never delivers is not warming up, and it
    // used never to be restarted at all: tower 111's camera sat like that for 9
    // minutes on 2026-09-24, capturing nothing and saying only "warming up".
    // CameraStall.h decides when waiting has become a stall, allowing for long
    // exposures, and when restarting has stopped helping.
    if (newest == 0) {
        const long long now    = nowMs();
        const long long waited = now - impl_->session_start_ms;
        const double exposure  = impl_->longestExposureUs();
        switch (onNoFirstFrame(waited, exposure, impl_->failed_restarts)) {
            case StallAction::Wait:
                if (now - impl_->last_no_frame_warn_ms > 5000) {
                    LOG_WARN << "camera: no frame yet (stream warming up)";
                    impl_->last_no_frame_warn_ms = now;
                }
                break;
            case StallAction::Restart:
                LOG_WARN << "camera: no frame at all " << waited / 1000
                         << " s into the stream; restarting it";
                ++impl_->failed_restarts;
                for (auto& st : impl_->stats) st->restarts.fetch_add(1);
                try {
                    impl_->stopAcquisition();
                    impl_->startAcquisition();
                } catch (const std::exception& e) {
                    LOG_WARN << "camera: acquisition restart failed: " << e.what();
                }
                break;
            case StallAction::Reset:
                LOG_ERROR << "camera: still no frame after " << impl_->failed_restarts
                          << " stream restarts; resetting the camera";
                for (size_t i = 0; i < impl_->cameras.size(); ++i) impl_->resetOne(i);
                impl_->failed_restarts = 0;
                break;
        }
        return false;
    }

    if (newest <= impl_->last_captured_ts) {
        LOG_TRACE_CAT(LogCat::Camera) << "stream stale; restarting acquisition";
        for (auto& s : impl_->stats) s->restarts.fetch_add(1);
        try {
            impl_->stopAcquisition();
            impl_->startAcquisition();
        } catch (const std::exception& e) {
            LOG_WARN << "camera: acquisition restart failed: " << e.what();
        }
        const long long deadline = nowMs() + 3000;
        while (impl_->newestTs() <= impl_->last_captured_ts && nowMs() < deadline)
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        if (impl_->newestTs() <= impl_->last_captured_ts &&
            onRestartFailed(++impl_->failed_restarts) == StallAction::Reset) {
            LOG_ERROR << "camera: the stream stays stale after " << impl_->failed_restarts
                      << " restarts; resetting the camera";
            for (size_t i = 0; i < impl_->cameras.size(); ++i) impl_->resetOne(i);
            impl_->failed_restarts = 0;
        }
    }

    bool any = false;
    for (auto& obs : impl_->observers) {
        Frame f;
        if (SP_DYN_CAST<FrameObserver>(obs)->takeLatest(f)) {
            LOG_TRACE_CAT(LogCat::Camera) << "capture cam" << f.cam_id << " ts=" << f.timestamp_ms;
            impl_->last_captured_ts = std::max(impl_->last_captured_ts, f.timestamp_ms);
            if (impl_->callback) impl_->callback(f);
            any = true;
            impl_->failed_restarts = 0;
        }
    }
    if (!any) {
        // Frames HAVE arrived this session (the newest == 0 case returned above), so
        // this is the restart-did-not-help path. The scheduler retries every tick,
        // so rate-limit.
        long long now = nowMs();
        if (now - impl_->last_no_frame_warn_ms > 5000) {
            LOG_WARN << "camera: stream stalled and did not recover after a restart";
            impl_->last_no_frame_warn_ms = now;
        }
    }
    return any;
}

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

int VimbaCameraSource::cameraCount() const { return static_cast<int>(impl_->cameras.size()); }

std::vector<CameraDeviceInfo> VimbaCameraSource::deviceInfo() {
    // Refresh the live sensor telemetry at most ~1 Hz - deviceInfo() is called every UI
    // tick (~10 Hz) but runs on the control thread (serialized with trigger()), and reading
    // camera features is not free. Identity + stats come from cached/atomic state.
    const long long now = nowMs();
    if (now - impl_->telemetry_ms > 1000) {
        impl_->telemetry_ms = now;
        for (size_t i = 0; i < impl_->cameras.size(); ++i) {
            auto&      cam = impl_->cameras[i];
            auto&      t   = impl_->telemetry[i];
            t.exposure_us    = getFloat(cam, "ExposureTime");
            t.gain_db        = getFloat(cam, "Gain");
            t.temperature_c  = getFloat(cam, "DeviceTemperature");
            t.actual_fps     = getFloat(cam, "AcquisitionFrameRate");
        }
    }

    std::vector<CameraDeviceInfo> out;
    out.reserve(impl_->cameras.size());
    for (size_t i = 0; i < impl_->cameras.size(); ++i) {
        const CamStats&        s = *impl_->stats[i];
        const Impl::Telemetry& t = impl_->telemetry[i];
        CameraDeviceInfo       d;
        d.id            = impl_->identity[i].id;
        d.model         = impl_->identity[i].model;
        d.serial        = impl_->identity[i].serial;
        d.streaming     = impl_->started && !impl_->observers.empty();
        d.width         = s.last_w.load();
        d.height        = s.last_h.load();
        d.payload_bytes = s.last_bytes.load();
        d.exposure_us   = t.exposure_us;
        d.gain_db       = t.gain_db;
        d.temperature_c = t.temperature_c;
        d.actual_fps    = t.actual_fps;
        d.frames_delivered = s.delivered.load();
        d.frames_dropped   = s.dropped.load();
        d.restarts         = s.restarts.load();
        out.push_back(std::move(d));
    }
    return out;
}

// ---- `test camera` -------------------------------------------------------------

bool VimbaCameraSource::grabFrame(int camera_index, Frame& out, int timeout_ms) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->observers.size()) return false;  // not streaming
    auto* obs = SP_DYN_CAST<FrameObserver>(impl_->observers[idx]).get();
    // Strictly newer than now: a frame that was already waiting was exposed
    // before the caller asked, possibly under settings it has since changed.
    const long long since    = nowMs();
    const long long deadline = since + timeout_ms;
    while (nowMs() < deadline) {
        if (obs->latestTimestamp() > since) return obs->takeLatest(out);
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    return false;
}

bool VimbaCameraSource::setFeature(int camera_index, const std::string& name,
                                   const std::string& value) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
    FeaturePtr         f;
    VmbFeatureDataType type = VmbFeatureDataUnknown;
    if (SP_ACCESS(impl_->cameras[idx])->GetFeatureByName(name.c_str(), f) != VmbErrorSuccess ||
        f->GetDataType(type) != VmbErrorSuccess)
        return false;
    // The value before the first change, so restoreProfile() can put it back
    // live. Only the first: a second change must not record the test's own value.
    auto& mine = impl_->touched[idx];
    const bool seen = std::any_of(mine.begin(), mine.end(),
                                  [&](const std::pair<std::string, std::string>& t) {
                                      return t.first == name;
                                  });
    if (!seen) {
        std::string before;
        switch (type) {
            case VmbFeatureDataFloat: {
                double v = 0;
                if (f->GetValue(v) == VmbErrorSuccess) {
                    char b[40]; std::snprintf(b, sizeof(b), "%.17g", v); before = b;
                }
                break;
            }
            case VmbFeatureDataInt: {
                VmbInt64_t v = 0;
                if (f->GetValue(v) == VmbErrorSuccess) before = std::to_string(v);
                break;
            }
            case VmbFeatureDataBool: {
                bool v = false;
                if (f->GetValue(v) == VmbErrorSuccess) before = v ? "true" : "false";
                break;
            }
            case VmbFeatureDataEnum:
            case VmbFeatureDataString: f->GetValue(before); break;
            default: break;
        }
        if (!before.empty()) mine.emplace_back(name, before);
    }
    auto write = [&]() -> VmbErrorType {
        try {
            switch (type) {
                case VmbFeatureDataFloat:  return f->SetValue(std::stod(value));
                case VmbFeatureDataInt:    return f->SetValue(static_cast<VmbInt64_t>(std::stoll(value)));
                case VmbFeatureDataBool:   return f->SetValue(value == "true" || value == "1");
                case VmbFeatureDataEnum:
                case VmbFeatureDataString: return f->SetValue(value.c_str());
                default:                   return VmbErrorWrongType;
            }
        } catch (const std::exception&) {
            return VmbErrorBadParameter;   // text that is not a number for a numeric feature
        }
    };
    VmbErrorType rc = write();
    // Some features cannot change while the camera streams - BlackLevel on the
    // Alvium 1800 answers InvalidAccess (measured on tower 121) where ExposureTime
    // and Gain are fine. Stop, write, and start again, as setProfile() does.
    if (rc == VmbErrorInvalidAccess && impl_->started) {
        try {
            impl_->stopAcquisition();
            rc = write();
            impl_->startAcquisition();
        } catch (const std::exception& e) {
            LOG_WARN << "camera: restarting the stream around " << name << " failed: " << e.what();
            rc = VmbErrorInternalFault;
        }
    }
    if (rc != VmbErrorSuccess) {
        LOG_WARN << "camera: setting " << name << " = " << value << " failed (error " << rc << ')';
        return false;
    }
    LOG_DEBUG << "camera cam" << camera_index << ": " << name << " = " << value;
    return true;
}

uint64_t VimbaCameraSource::frameSeq() {
    if (impl_->stats.empty()) return 0;
    std::lock_guard<std::mutex> lock(impl_->stats[0]->recent_mutex);
    return impl_->stats[0]->seq;
}

std::vector<FrameStats> VimbaCameraSource::framesSince(uint64_t after) {
    std::vector<FrameStats> out;
    if (impl_->stats.empty()) return out;
    std::lock_guard<std::mutex> lock(impl_->stats[0]->recent_mutex);
    for (const auto& f : impl_->stats[0]->recent)
        if (f.seq > after) out.push_back(f);
    return out;
}

bool VimbaCameraSource::readFeature(int camera_index, const std::string& name, double& value) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
    FeaturePtr         f;
    VmbFeatureDataType type = VmbFeatureDataUnknown;
    if (SP_ACCESS(impl_->cameras[idx])->GetFeatureByName(name.c_str(), f) != VmbErrorSuccess ||
        f->GetDataType(type) != VmbErrorSuccess)
        return false;
    if (type == VmbFeatureDataFloat) return f->GetValue(value) == VmbErrorSuccess;
    if (type == VmbFeatureDataInt) {
        VmbInt64_t v = 0;
        if (f->GetValue(v) != VmbErrorSuccess) return false;
        value = static_cast<double>(v);
        return true;
    }
    return false;
}

bool VimbaCameraSource::featureRange(int camera_index, const std::string& name, double& min,
                                     double& max) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
    FeaturePtr         f;
    VmbFeatureDataType type = VmbFeatureDataUnknown;
    if (SP_ACCESS(impl_->cameras[idx])->GetFeatureByName(name.c_str(), f) != VmbErrorSuccess ||
        f->GetDataType(type) != VmbErrorSuccess)
        return false;
    if (type == VmbFeatureDataFloat) return f->GetRange(min, max) == VmbErrorSuccess;
    if (type == VmbFeatureDataInt) {
        VmbInt64_t a = 0, b = 0;
        if (f->GetRange(a, b) != VmbErrorSuccess) return false;
        min = static_cast<double>(a);
        max = static_cast<double>(b);
        return true;
    }
    return false;
}

bool VimbaCameraSource::restoreProfile(int camera_index) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->profiles.size()) return false;

    // Put back what setFeature() changed, live and in reverse order - values
    // first, then the auto modes that take over from them. No stream restart:
    // stopping and starting it repeatedly is exactly what preceded tower 111's
    // camera getting stuck (known-issues #15).
    auto undo = std::move(impl_->touched[idx]);
    impl_->touched[idx].clear();
    bool ok = !undo.empty();
    for (auto it = undo.rbegin(); ok && it != undo.rend(); ++it) {
        FeaturePtr         f;
        VmbFeatureDataType type = VmbFeatureDataUnknown;
        if (SP_ACCESS(impl_->cameras[idx])->GetFeatureByName(it->first.c_str(), f) != VmbErrorSuccess ||
            f->GetDataType(type) != VmbErrorSuccess) { ok = false; break; }
        VmbErrorType rc = VmbErrorWrongType;
        try {
            switch (type) {
                case VmbFeatureDataFloat:  rc = f->SetValue(std::stod(it->second)); break;
                case VmbFeatureDataInt:    rc = f->SetValue(static_cast<VmbInt64_t>(std::stoll(it->second))); break;
                case VmbFeatureDataBool:   rc = f->SetValue(it->second == "true"); break;
                case VmbFeatureDataEnum:
                case VmbFeatureDataString: rc = f->SetValue(it->second.c_str()); break;
                default: break;
            }
        } catch (const std::exception&) { rc = VmbErrorBadParameter; }
        ok = rc == VmbErrorSuccess;
    }
    if (ok) {
        LOG_INFO << "camera: " << undo.size() << " setting(s) put back live";
        return true;
    }
    // Nothing recorded, or a write refused: the whole profile, the slow way.
    // setProfile() cycles the stream when one is running, since a profile may
    // carry features that are not writable live.
    return setProfile(camera_index, impl_->profiles[idx]);
}

bool VimbaCameraSource::resetCamera(int camera_index) {
    const size_t idx = static_cast<size_t>(camera_index);
    if (camera_index < 0 || idx >= impl_->cameras.size()) return false;
    const bool ok = impl_->resetOne(idx);
    impl_->failed_restarts = 0;
    return ok;
}

}  // namespace fgc