fwt_software/src/camera/ImagePipeline.cpp

#include "fgc/ImagePipeline.h"

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

#include <filesystem>

#include <opencv2/opencv.hpp>

namespace fs = std::filesystem;

namespace fgc {
namespace {

// Maps a clockwise rotation in degrees to the cv::rotate code. Returns false
// when there is nothing to do (0 deg, or an unexpected value - ConfigLoader
// rejects anything but 0/90/180/270 before it reaches here).
bool rotateCode(int deg, int& code) {
    switch (deg) {
        case 90:  code = cv::ROTATE_90_CLOCKWISE;        return true;
        case 180: code = cv::ROTATE_180;                 return true;
        case 270: code = cv::ROTATE_90_COUNTERCLOCKWISE; return true;
        default:  return false;
    }
}

}  // namespace

ImagePipeline::ImagePipeline(IControlChannel& channel, std::function<Orientation()> orientation,
                             Params params)
    : channel_(channel), orientation_(std::move(orientation)), params_(std::move(params)) {
    display_.store(params_.display);
}

ImagePipeline::~ImagePipeline() { stop(); }

void ImagePipeline::start() {
    if (running_.exchange(true)) return;
    worker_ = std::thread(&ImagePipeline::run, this);
}

void ImagePipeline::stop() {
    if (!running_.exchange(false)) return;
    cv_.notify_all();
    if (worker_.joinable()) worker_.join();
}

void ImagePipeline::submit(const Frame& frame) {
    // Sample the pose now, while the frame is fresh, and carry it through the
    // queue. Both sources behind this callback are mutex-guarded snapshot reads,
    // so it costs the camera's delivery thread a lock and a copy.
    QueuedFrame q{frame, orientation_ ? orientation_() : Orientation{}};
    {
        std::lock_guard<std::mutex> lock(mutex_);
        queue_.push(std::move(q));
    }
    cv_.notify_one();
}

void ImagePipeline::run() {
    while (running_) {
        QueuedFrame queued;
        {
            std::unique_lock<std::mutex> lock(mutex_);
            cv_.wait(lock, [this] { return !queue_.empty() || !running_; });
            if (!running_ && queue_.empty()) break;
            queued = std::move(queue_.front());
            queue_.pop();
        }
        process(queued);
    }
}

const ImagePipeline::CameraOutput& ImagePipeline::outputFor(int cam_id) const {
    if (cam_id >= 0 && cam_id < static_cast<int>(params_.cameras.size()))
        return params_.cameras[cam_id];
    // A source delivering an index we were not told about still has to write
    // somewhere identifiable rather than colliding with camera 0.
    static const CameraOutput fallback{"cam?", 0, 0.8, 4};
    return fallback;
}

void ImagePipeline::process(const QueuedFrame& queued) {
    const Frame& frame = queued.frame;
    if (frame.channels != 1 && frame.channels != 3) {
        LOG_WARN << "Dropping frame with unsupported channel count " << frame.channels;
        return;
    }

    const CameraOutput& out = outputFor(frame.cam_id);

    const int type = frame.channels == 1 ? CV_8UC1 : CV_8UC3;
    cv::Mat src(frame.height, frame.width, type, const_cast<uint8_t*>(frame.data.data()));
    cv::Mat rotated;
    int  code   = 0;
    bool rotate = rotateCode(out.rotation_deg, code);
    if (rotate) cv::rotate(src, rotated, code);
    // src only wraps frame.data, so an unrotated frame costs no copy at all.
    const cv::Mat& img = rotate ? rotated : src;

    if (display_.load()) {
        // This runs on the worker thread, so anything escaping here terminates the
        // whole program - and OpenCV's GUI calls throw whenever there is no usable
        // display: no X/Wayland, DISPLAY unset or pointing at a dead server (an SSH
        // session without X11 forwarding), or a highgui built with no GUI backend.
        // A preview window is never worth taking a tower down for, so failure
        // disables it and capture carries on untouched.
        try {
            cv::namedWindow("Display Image", cv::WINDOW_NORMAL);
            cv::resizeWindow("Display Image", img.cols / 4, img.rows / 4);
            cv::imshow("Display Image", img);
            cv::waitKey(10);
        } catch (const std::exception& e) {
            display_.store(false);
            LOG_WARN << "local display unavailable; preview disabled (capture is unaffected): "
                     << e.what();
        } catch (...) {
            display_.store(false);
            LOG_WARN << "local display failed for an unknown reason; preview disabled "
                        "(capture is unaffected)";
        }
    }

    const std::string label = out.label.empty() ? ("cam" + std::to_string(frame.cam_id))
                                                : out.label;
    const fs::path     dir   = fs::path(params_.output_dir) / label;
    std::error_code    ec;
    fs::create_directories(dir, ec);
    if (ec) {
        LOG_ERROR << "Cannot create capture dir " << dir.string() << ": " << ec.message();
        return;
    }
    const fs::path file = dir / (std::to_string(frame.timestamp_ms) + ".jxl");

    if (params_.demo) {
        fs::copy_file(params_.demo_image, file, fs::copy_options::overwrite_existing, ec);
        if (ec) LOG_ERROR << "Demo copy failed: " << ec.message();
    } else {
        bool ok = JpegXlEncoder::encodeToFile(file.string(), img.data, img.cols, img.rows,
                                              frame.channels, out.jxl_distance,
                                              out.jxl_effort);
        if (!ok) {
            LOG_ERROR << "Failed to encode " << file.string();
            return;
        }
    }

    // Sampled at submit(), not here: by this point the encode above has run and
    // the gimbal may already be slewing to the next waypoint.
    const Orientation& o = queued.orientation;
    CamEvent ev;
    ev.tower           = params_.tower;
    ev.camera          = label;
    ev.heading_decideg = static_cast<int>(o.yaw_deg * 10);
    ev.pitch_decideg   = static_cast<int>(o.pitch_deg * 10);
    ev.timestamp_ms    = frame.timestamp_ms;
    channel_.publishCamEvent(ev);
    {
        std::lock_guard<std::mutex> lock(last_event_mutex_);
        last_event_ = ev;
    }

    saved_.fetch_add(1);
    LOG_DEBUG << "Saved " << file.string() << " (" << img.cols << "x" << img.rows << ")";
}

std::optional<CamEvent> ImagePipeline::lastEvent() const {
    std::lock_guard<std::mutex> lock(last_event_mutex_);
    return last_event_;
}

}  // namespace fgc