fwt_software/src/camera/CompositeCameraSource.cpp

#include "fgc/CompositeCameraSource.h"

#include "fgc/Logger.h"

#include <chrono>
#include <stdexcept>

namespace fgc {

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

CompositeCameraSource::CompositeCameraSource(std::vector<std::unique_ptr<ICameraSource>> children,
                                             std::vector<int> counts, int follower_wait_ms)
    : children_(std::move(children)), counts_(std::move(counts)),
      follower_wait_ms_(follower_wait_ms) {
    if (children_.empty()) throw std::invalid_argument("CompositeCameraSource needs a camera");
    if (counts_.empty())
        for (const auto& c : children_) counts_.push_back(c->cameraCount());
    if (counts_.size() != children_.size())
        throw std::invalid_argument("CompositeCameraSource: one camera count per source");
    int next = 0;
    for (int n : counts_) {
        offsets_.push_back(next);
        next += n;
    }
}

ICameraSource* CompositeCameraSource::route(int camera_index, int& local) const {
    for (size_t k = children_.size(); k-- > 0;) {
        if (camera_index < offsets_[k]) continue;
        local = camera_index - offsets_[k];
        return local < counts_[k] ? children_[k].get() : nullptr;
    }
    return nullptr;
}

void CompositeCameraSource::open() {
    // The leader failing stops the run, as a lone camera would. A follower
    // failing does not: the scan goes on without its pictures.
    children_[0]->open();
    for (size_t k = 1; k < children_.size(); ++k) {
        try {
            children_[k]->open();
        } catch (const std::exception& e) {
            LOG_ERROR << "camera " << offsets_[k] << " did not open (" << e.what()
                      << "); continuing without it";
        }
    }
}

void CompositeCameraSource::close() {
    for (auto& c : children_) c->close();
}

void CompositeCameraSource::start() {
    for (auto& c : children_) c->start();
}

void CompositeCameraSource::stop() {
    for (auto& c : children_) c->stop();
}

bool CompositeCameraSource::trigger() {
    bool followers_ready = true;
    for (size_t k = 1; k < children_.size(); ++k) followers_ready &= children_[k]->frameReady();
    if (!followers_ready) {
        const long long now = steadyMs();
        if (waiting_since_ == 0) waiting_since_ = now;
        if (now - waiting_since_ < follower_wait_ms_) return false;   // asked again next tick
        LOG_DEBUG << "camera: a follower had no fresh frame for " << now - waiting_since_
                  << " ms; taking the picture without it";
    }
    waiting_since_ = 0;
    const bool taken = children_[0]->trigger();
    if (taken)
        for (size_t k = 1; k < children_.size(); ++k) children_[k]->trigger();
    return taken;
}

void CompositeCameraSource::moving() {
    for (auto& c : children_) c->moving();
}

bool CompositeCameraSource::frameReady() {
    for (auto& c : children_)
        if (!c->frameReady()) return false;
    return true;
}

void CompositeCameraSource::setFrameCallback(FrameCallback cb) {
    callback_ = std::move(cb);
    // The leader's frames pass through untouched (they can be 24 MB); its
    // camera 0 is global camera 0, so not even the index changes.
    children_[0]->setFrameCallback([this](const Frame& f) {
        if (callback_) callback_(f);
    });
    for (size_t k = 1; k < children_.size(); ++k) {
        const int offset = offsets_[k];
        children_[k]->setFrameCallback([this, offset](const Frame& f) {
            if (!callback_) return;
            Frame g  = f;
            g.cam_id = f.cam_id + offset;
            callback_(g);
        });
    }
}

bool CompositeCameraSource::setProfile(int camera_index, const CameraProfile& profile) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->setProfile(local, profile);
}

uint64_t CompositeCameraSource::frameSeq() { return children_[0]->frameSeq(); }

std::vector<FrameStats> CompositeCameraSource::framesSince(uint64_t after) {
    return children_[0]->framesSince(after);
}

bool CompositeCameraSource::readFeature(int camera_index, const std::string& name,
                                        double& value) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->readFeature(local, name, value);
}

// What the children have opened, so a camera missing from the bus shows as a
// shortfall (HardwarePresence) - while the indices stay those configured.
int CompositeCameraSource::cameraCount() const {
    int n = 0;
    for (const auto& c : children_) n += c->cameraCount();
    return n;
}

std::vector<CameraDeviceInfo> CompositeCameraSource::deviceInfo() {
    std::vector<CameraDeviceInfo> out;
    for (auto& c : children_) {
        auto d = c->deviceInfo();
        out.insert(out.end(), d.begin(), d.end());
    }
    return out;
}

std::string CompositeCameraSource::offline(int camera_index) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c ? c->offline(local) : "no such camera";
}

bool CompositeCameraSource::grabFrame(int camera_index, Frame& out, int timeout_ms) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    if (!c || !c->grabFrame(local, out, timeout_ms)) return false;
    out.cam_id = camera_index;
    return true;
}

bool CompositeCameraSource::setFeature(int camera_index, const std::string& name,
                                       const std::string& value) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->setFeature(local, name, value);
}

bool CompositeCameraSource::featureRange(int camera_index, const std::string& name, double& min,
                                         double& max) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->featureRange(local, name, min, max);
}

bool CompositeCameraSource::restoreProfile(int camera_index) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->restoreProfile(local);
}

bool CompositeCameraSource::resetCamera(int camera_index) {
    int  local = 0;
    auto* c    = route(camera_index, local);
    return c && c->resetCamera(local);
}

}  // namespace fgc