// Copyright (c) French Touch Factory. All Rights Reserved.
//
// Ithaca RGBD preview server — Jetson.
//
// Serves every camera on this node over one TCP connection: colour as the
// sensor's own JPEG, forwarded untouched, and depth as the sensor's own 16-bit
// millimetres, losslessly compressed.
//
// Why not the H.264/WebRTC path it replaces:
//
//   Depth is data, not a picture. Squeezing it into 8 bits for a video codec put
//   a floor of 19.6 mm on precision before compression even began, and H.264's
//   smoothing then bled object edges into the background — a geometric error,
//   not a cosmetic one. Sent as-is it is exact, and LZ4 over separated byte
//   planes costs 3.35 ms a frame for a 2.95x ratio (measured, 640x576).
//
//   Colour is already JPEG when it leaves the sensor. Decoding it to re-encode
//   as H.264 spent NVDEC and NVENC, added a generation of loss, and made the two
//   cameras contend for one decoder — which is where their differing latency
//   came from. Forwarded as-is it costs nothing.
//
//   Colour and depth of one frameset leave in the same message, under the same
//   lock. They cannot drift apart. Over WebRTC they were two sessions with two
//   jitter buffers and no shared clock, and MediaMTX's WHEP serves only one video
//   track per session, so putting them in one was not available either.
//
//   And nothing here buffers: no rate-smoothing window, no group of pictures, no
//   jitter buffer. When the link falls behind, whole frames are dropped instead,
//   which is safe because every frame stands alone.
//
// The cost of all this is bandwidth — roughly 55 Mbit/s per camera binned — so
// it is a local-network path. WebRTC remains the answer for anything leaving the
// site.
//
// Usage:
//   ithaca_rgbd_server --map <serial>:<index>[,<serial>:<index>...]
//
//   --map is both the numbering AND the selection: a camera named there is
//   served under that index, and a camera absent from a NON-EMPTY map is not
//   streamed. An empty (or missing) map serves every camera, numbered by
//   enumeration order — what a run by hand relies on.
//
//   --map-file names a file holding that same text, re-read once a second, so the
//   composition can change WITHOUT restarting: a camera dropped from it has its
//   streams stopped, one added has them started, and the others keep running
//   throughout. That is how the operator's per-camera checkbox reaches the sensor
//   while a preview is live — restarting instead took every camera of the node
//   down for about ten seconds.
//
//   A SECOND line of that file, if present, is the alignment: "raw" or "compare".
//   Those two share their stream configuration, so the node can be told to start
//   or stop producing the SDK's own transform at any moment. The other modes are
//   fixed for the life of the process, needing a different colour format or a
//   different target, and are computed by the viewer anyway.
//
//   Stopping the streams is what saves the USB bandwidth, the depth engine and the
//   heat. The device handle stays open, so turning the camera back on is immediate;
//   Record does not care, since it stops this server before opening its own
//   recorders.
//                      [--depth-mode M] [--color-mode M] [--rate N]
//                      [--align raw|d2c|d2c-fov|c2d|compare] [--port P]
#include <libobsensor/ObSensor.hpp>
// Not pulled in by ObSensor.hpp, and this is where the ray table lives.
#include <libobsensor/hpp/Utils.hpp>
#include <lz4.h>

#include <stdio.h>      // jpeglib.h needs FILE declared before it
#include <jpeglib.h>

#include <arpa/inet.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <unistd.h>

#include <atomic>
#include <chrono>
#include <csignal>
#include <cstdint>
#include <cstring>
#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <memory>
#include <mutex>
#include <set>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>

namespace {

std::atomic<bool> g_running{true};
void onSignal(int) { g_running = false; }

uint64_t nowUs() {
    return std::chrono::duration_cast<std::chrono::microseconds>(
               std::chrono::steady_clock::now().time_since_epoch()).count();
}

// Interleaved 16-bit samples compress poorly: the high bytes are smooth and
// repetitive, the low bytes are mostly sensor noise, and mixed together they
// spoil each other's matches. Separated, each compresses on its own terms —
// measured 2.95x against 2.27x, and faster with it.
void splitPlanes(const uint8_t *src, size_t bytes, std::vector<uint8_t> &out) {
    const size_t n = bytes / 2;
    out.resize(bytes);
    for(size_t i = 0; i < n; i++) {
        out[i]     = src[2 * i + 1];
        out[n + i] = src[2 * i];
    }
}

// Re-encodes a warped colour frame, which only C2D needs: in every other mode
// the colour that arrives is already a JPEG and is forwarded as it is.
//
// One of these per camera, kept alive across frames — jpeg_create_compress
// allocates its tables and its Huffman state, and building them thirty times a
// second per camera would cost more than the encode.
class JpegEncoder {
public:
    JpegEncoder() {
        info_.err = jpeg_std_error(&err_);
        jpeg_create_compress(&info_);
    }

    ~JpegEncoder() {
        jpeg_destroy_compress(&info_);
        if(buffer_ != nullptr) free(buffer_);
    }

    JpegEncoder(const JpegEncoder &) = delete;
    JpegEncoder &operator=(const JpegEncoder &) = delete;

    /// Encodes packed 24-bit RGB. Returns false and leaves size at 0 on failure.
    /// The returned pointer belongs to this encoder and is valid until the next
    /// call, which is enough: the frame is sent before the next one is encoded.
    bool encode(const uint8_t *rgb, int width, int height, int quality,
                const uint8_t *&data, size_t &size) {
        data = nullptr;
        size = 0;

        // jpeg_mem_dest grows this itself and hands back the grown pointer, so it
        // is kept between calls and settles at the largest frame seen.
        jpeg_mem_dest(&info_, &buffer_, &capacity_);

        info_.image_width      = static_cast<JDIMENSION>(width);
        info_.image_height     = static_cast<JDIMENSION>(height);
        info_.input_components = 3;
        info_.in_color_space   = JCS_RGB;
        jpeg_set_defaults(&info_);
        jpeg_set_quality(&info_, quality, TRUE);

        jpeg_start_compress(&info_, TRUE);
        const int stride = width * 3;
        while(info_.next_scanline < info_.image_height) {
            JSAMPROW row = const_cast<JSAMPROW>(rgb + info_.next_scanline * stride);
            jpeg_write_scanlines(&info_, &row, 1);
        }
        jpeg_finish_compress(&info_);

        if(buffer_ == nullptr || capacity_ == 0) return false;
        data = buffer_;
        size = capacity_;
        return true;
    }

private:
    jpeg_compress_struct info_{};
    jpeg_error_mgr err_{};
    uint8_t *buffer_ = nullptr;
    unsigned long capacity_ = 0;
};

bool sendAll(int fd, const void *data, size_t size) {
    const uint8_t *p = static_cast<const uint8_t *>(data);
    while(size > 0) {
        const ssize_t n = ::send(fd, p, size, MSG_NOSIGNAL);
        if(n <= 0) return false;
        p += n;
        size -= static_cast<size_t>(n);
    }
    return true;
}

// Bytes the kernel has still to put on the wire. On a link that cannot keep up
// this is what grows, and with it the delay: TCP queues frames happily until the
// picture is seconds behind. A preview must drop instead.
bool backedUp(int fd, int limitBytes) {
    int pending = 0;
    if(::ioctl(fd, TIOCOUTQ, &pending) != 0) return false;
    return pending > limitBytes;
}

// A camera's calibration, as it goes on the wire: enough for the receiver to put
// a depth pixel on the colour image itself, and nothing more.
//
// What it holds, and why exactly this:
//
//   ray table   two floats per depth pixel, the direction that pixel looks in.
//               The depth camera's distortion is already solved into it. It has
//               to be, because that model cannot be inverted at the far end —
//               k1 = 17.4 on this sensor, strong enough that an iterative
//               inverse was 10 pixels out at the edges while looking perfect in
//               the middle, and the SDK itself refuses to evaluate the corners.
//
//   colour intrinsics and the rigid transform. No distortion coefficients: the
//               SDK's own projection applies none, and reproducing it exactly —
//               0.000 pixel over 123 points across the whole image at three
//               distances — means a plain pinhole. Applying the stored colour
//               coefficients, or inverting them, was 9 pixels out.
//
// The receiver then does, per depth pixel: P = ray * millimetres, P' = R P + T,
// and u = P'.x / P'.z * fx + cx. That is a texture read and a few multiplies —
// which is the whole point of sending this instead of transforming here.
#pragma pack(push, 1)
struct WireCalibrationHeader {
    int32_t depthWidth, depthHeight;
    int32_t colourWidth, colourHeight;
    float   colourFx, colourFy, colourCx, colourCy;
    float   rot[9];             // depth -> colour, row major
    float   trans[3];           // depth -> colour, in millimetres
    int32_t rayTableBytes;      // LZ4 payload that follows this header
    int32_t rayTablePlainBytes; // what it expands to: depth pixels * 2 floats
};
#pragma pack(pop)

// The viewers currently connected. A frameset is written to each in turn under
// one lock, so no two cameras can interleave their messages on any of them.
struct Viewers {
    std::mutex gate;
    std::vector<int> fds;

    void add(int fd) {
        std::lock_guard<std::mutex> guard(gate);
        fds.push_back(fd);
    }

    std::vector<int> snapshot() {
        std::lock_guard<std::mutex> guard(gate);
        return fds;
    }

    void drop(int fd) {
        std::lock_guard<std::mutex> guard(gate);
        for(size_t i = 0; i < fds.size(); i++) {
            if(fds[i] == fd) {
                fds.erase(fds.begin() + static_cast<long>(i));
                ::close(fd);
                return;
            }
        }
    }

    void closeAll() {
        std::lock_guard<std::mutex> guard(gate);
        for(int fd : fds) ::close(fd);
        fds.clear();
    }

    size_t count() {
        std::lock_guard<std::mutex> guard(gate);
        return fds.size();
    }
};

#pragma pack(push, 1)
struct Header {
    uint32_t magic;       // 'ITHD'
    uint16_t camera;      // camIndex, as the Recorder panel numbers it
    uint16_t kind;        // 0 = depth (LZ4, split planes), 1 = colour (JPEG),
                          // 2 = calibration (once per viewer), 3 = the SDK's own
                          // depth-to-colour result (LZ4, split planes)
    uint16_t width;
    uint16_t height;
    uint32_t payload;     // bytes that follow
    uint32_t ageUs;       // age of the frame when it was sent
    uint32_t sequence;    // shared by the colour and depth of one frameset
};
#pragma pack(pop)

const uint32_t kMagic = 0x44485449;   // 'ITHD' little-endian

// Only C2D re-encodes, and this is the one place a loss is introduced anywhere
// in this server. High enough that the artefacts stay below what the sensor's
// own JPEG already has, since a second generation is what is being paid for.
const int kJpegQuality = 90;

struct Mode { int width, height; };

bool depthMode(const std::string &name, Mode &out) {
    if(name == "NFOV_2X2BINNED") { out = {320, 288};   return true; }
    if(name == "NFOV_UNBINNED")  { out = {640, 576};   return true; }
    if(name == "WFOV_2X2BINNED") { out = {512, 512};   return true; }
    if(name == "WFOV_UNBINNED")  { out = {1024, 1024}; return true; }
    return false;
}

// Which grid the two images are put on before they leave.
//
//   Raw     each sensor's own view. The two sit a couple of centimetres apart,
//           so a given object falls on different pixels in the two images and
//           nothing downstream can pair them without the calibration. Costs
//           nothing, and the colour is the sensor's own JPEG untouched.
//
//   D2C     the depth resampled into the colour camera's view, at the colour
//           resolution: pixel (x, y) is the same point of the world in both.
//           This is k4a_transformation_depth_image_to_color_camera — a mesh warp
//           rather than a per-pixel reprojection, which is what keeps holes out
//           of the result and resolves occlusions with a z-buffer. The colour
//           still leaves untouched; only the depth is transformed, and it grows
//           to the colour resolution (4 MB a frame at 1080p, before compression).
//
//   D2CFov  the same view and the same aspect, kept at the depth sensor's own
//           resolution. K4A has no such variant; this one is the v2 SDK's own
//           (Align::setMatchTargetResolution). Texture coordinates still line up,
//           which is what a shader or a point cloud actually samples with; only
//           per-pixel identity is given up, and with it most of the bandwidth.
//
//   C2D     the colour resampled into the depth camera's view, at the depth
//           resolution — k4a_transformation_color_image_to_depth_camera. This is
//           the expensive one and unavoidably so: the sensor puts MJPG on the
//           USB, so the frame has to be decoded to be warped, and then encoded
//           again to be sent. Raw and D2C both avoid touching the colour at all.
//   Compare  the raw depth as in Raw, plus the SDK's own depth-to-colour result
//            alongside it, so a viewer can overlay its own transform on the
//            reference and see where the two place their points differently.
//            Costs what the transform costs — 22 frames a second instead of 29,
//            and a second depth map on the wire. For diagnosis, not capture.
enum class Align { Raw, D2C, D2CFov, C2D, Compare };

bool alignMode(const std::string &name, Align &out) {
    if(name == "raw")     { out = Align::Raw;     return true; }
    if(name == "d2c")     { out = Align::D2C;     return true; }
    if(name == "d2c-fov") { out = Align::D2CFov;  return true; }
    if(name == "c2d")     { out = Align::C2D;     return true; }
    if(name == "compare") { out = Align::Compare; return true; }
    return false;
}

// For messages about the mode in force NOW, which is not necessarily the one named
// on the command line.
const char *alignText(Align a) {
    switch(a) {
        case Align::Raw:     return "raw";
        case Align::D2C:     return "d2c";
        case Align::D2CFov:  return "d2c-fov";
        case Align::C2D:     return "c2d";
        case Align::Compare: return "compare";
    }
    return "?";
}

bool colorMode(const std::string &name, Mode &out) {
    if(name == "720p")  { out = {1280, 720};  return true; }
    if(name == "1080p") { out = {1920, 1080}; return true; }
    if(name == "1440p") { out = {2560, 1440}; return true; }
    if(name == "1536p") { out = {2048, 1536}; return true; }
    if(name == "2160p") { out = {3840, 2160}; return true; }
    return false;
}

// "serial:index,serial:index" — the bridge owns these numbers, and they name the
// slot each stream occupies in the mosaic.
std::map<std::string, uint16_t> parseMap(const std::string &text) {
    std::map<std::string, uint16_t> out;
    size_t start = 0;
    while(start < text.size()) {
        const size_t comma = text.find(',', start);
        const std::string item = text.substr(start, comma - start);
        const size_t colon = item.find(':');
        if(colon != std::string::npos) {
            out[item.substr(0, colon)] =
                static_cast<uint16_t>(std::atoi(item.substr(colon + 1).c_str()));
        }
        if(comma == std::string::npos) break;
        start = comma + 1;
    }
    return out;
}

// What the node should be doing right now: which cameras, and in which mode.
//
// Read on a timer rather than pushed, so nothing has to be added to the viewer
// protocol and a crashed writer cannot leave the server waiting on a command that
// never comes. Both live in ONE file so a single atomic rename changes them
// together — they can never be read half-updated against each other.
struct Composition {
    std::map<std::string, uint16_t> indices;
    Align align = Align::Raw;
    bool hasAlign = false;
};

Composition readComposition(const std::string &path) {
    Composition out;
    std::ifstream in(path);
    if(!in) return out;

    std::string line;
    std::getline(in, line);
    out.indices = parseMap(line);

    // Second line optional: a writer that does not know about it simply leaves the
    // mode as the command line set it.
    if(std::getline(in, line)) {
        while(!line.empty() && (line.back() == '\r' || line.back() == ' ')) line.pop_back();
        Align parsed{};
        if(!line.empty() && alignMode(line, parsed)) {
            out.align = parsed;
            out.hasAlign = true;
        }
    }
    return out;
}

// One camera the server can turn on and off in place. The frame callback is kept
// here because start() needs it again on every restart of this one pipeline.
struct Cam {
    std::string serial;
    uint16_t camIndex = 0;
    std::shared_ptr<ob::Pipeline> pipeline;
    std::shared_ptr<ob::Config> config;
    std::function<void(std::shared_ptr<ob::FrameSet>)> callback;
    bool running = false;
};

}  // namespace

int main(int argc, char **argv) {
    std::string mapText, mapFile, depthName = "NFOV_2X2BINNED", colorName = "720p",
                alignName = "none";
    int port = 5020, rate = 30, backlogLimit = 1 << 20;

    for(int i = 1; i < argc; i++) {
        const std::string arg = argv[i];
        if(arg == "--map"        && i + 1 < argc) mapText   = argv[++i];
        else if(arg == "--map-file"   && i + 1 < argc) mapFile   = argv[++i];
        else if(arg == "--depth-mode" && i + 1 < argc) depthName = argv[++i];
        else if(arg == "--color-mode" && i + 1 < argc) colorName = argv[++i];
        else if(arg == "--align"      && i + 1 < argc) alignName = argv[++i];
        else if(arg == "--rate"       && i + 1 < argc) rate      = std::atoi(argv[++i]);
        else if(arg == "--port"       && i + 1 < argc) port      = std::atoi(argv[++i]);
        else if(arg == "--backlog"    && i + 1 < argc) backlogLimit = std::atoi(argv[++i]);
        else {
            std::cerr << "unknown option: " << arg << std::endl;
            return 2;
        }
    }

    Mode depth{}, colour{};
    if(!depthMode(depthName, depth)) {
        std::cerr << "unknown depth mode: " << depthName << std::endl;
        return 2;
    }
    if(!colorMode(colorName, colour)) {
        std::cerr << "unknown colour mode: " << colorName << std::endl;
        return 2;
    }
    Align align{};
    if(!alignMode(alignName, align)) {
        std::cerr << "unknown alignment: " << alignName
                  << " (expected none, color or color-fov)" << std::endl;
        return 2;
    }
    std::map<std::string, uint16_t> indices = parseMap(mapText);
    // The file wins when it is there and readable: it is the live truth, and --map
    // is then only the starting point the bridge passed on the command line.
    if(!mapFile.empty()) {
        const Composition start = readComposition(mapFile);
        if(!start.indices.empty()) indices = start.indices;
        if(start.hasAlign) align = start.align;
    }

    // Only these two can be switched under way: they share the stream
    // configuration, so nothing has to be reopened. Fixed otherwise.
    const bool liveSwitch = (align == Align::Raw || align == Align::Compare);
    std::atomic<int> liveAlign{static_cast<int>(align)};

    std::signal(SIGINT, onSignal);
    std::signal(SIGTERM, onSignal);

    // ---- listening socket ---------------------------------------------------
    const int listenFd = ::socket(AF_INET, SOCK_STREAM, 0);
    int yes = 1;
    ::setsockopt(listenFd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));

    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = INADDR_ANY;
    addr.sin_port = htons(static_cast<uint16_t>(port));
    if(::bind(listenFd, reinterpret_cast<sockaddr *>(&addr), sizeof(addr)) != 0) {
        std::cerr << "cannot bind port " << port << std::endl;
        return 1;
    }
    ::listen(listenFd, 2);

    Viewers viewers;
    std::thread accepter([&]() {
        while(g_running) {
            const int fd = ::accept(listenFd, nullptr, nullptr);
            if(fd < 0) break;
            // Send at once rather than let Nagle group small writes: every
            // millisecond held here is a millisecond of preview delay.
            ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &yes, sizeof(yes));
            viewers.add(fd);
            std::cout << "Viewer connected (" << viewers.count() << " watching)."
                      << std::endl;
        }
    });

    // ---- cameras ------------------------------------------------------------
    ob::Context::setExtensionsDirectory(OB_EXTENSIONS_DIR);
    ob::Context ctx;
    auto list = ctx.queryDeviceList();
    if(list->getCount() == 0) {
        std::cerr << "no camera found" << std::endl;
        // The accepter is still joinable, and letting a joinable thread reach its
        // destructor calls std::terminate — an abort on the ordinary path where
        // nothing is plugged in.
        ::shutdown(listenFd, SHUT_RDWR);
        ::close(listenFd);
        accepter.detach();
        return 1;
    }

    std::mutex sendLock;
    std::vector<Cam> cams;
    std::vector<std::shared_ptr<std::atomic<uint32_t>>> dropCounters;

    for(uint32_t i = 0; i < list->getCount(); i++) {
        auto dev = list->getDevice(i);
        const std::string serial = list->getSerialNumber(i);

        // Fall back to enumeration order only if the bridge did not say: its
        // numbering is what the operator sees and edits.
        uint16_t camIndex = static_cast<uint16_t>(i);
        auto known = indices.find(serial);
        if(known != indices.end()) camIndex = known->second;

        // A non-empty map is the list to STREAM. The pipeline is still built for
        // the others, so that a camera ticked back on starts at once instead of
        // paying for a device open; what an unticked camera must not do is stream,
        // which is where the USB bandwidth, the depth engine and the heat go.
        const bool wanted = indices.empty() || known != indices.end();

        auto pipeline = std::make_shared<ob::Pipeline>(dev);
        auto config   = std::make_shared<ob::Config>();
        std::shared_ptr<ob::VideoStreamProfile> colourProfile, depthProfile;
        try {
            depthProfile = pipeline->getStreamProfileList(OB_SENSOR_DEPTH)
                                   ->getVideoStreamProfile(depth.width, depth.height,
                                                           OB_FORMAT_Y16, rate);
            config->enableStream(depthProfile);
            // MJPG everywhere except C2D: the sensor puts JPEG on the USB and
            // that is exactly what we want to forward. C2D has to warp the
            // colour, so it needs pixels, and asking the SDK for RGB is what
            // makes it decode — there is no way to warp a JPEG.
            const OBFormat colourFormat = (align == Align::C2D) ? OB_FORMAT_RGB
                                                                : OB_FORMAT_MJPG;
            colourProfile = pipeline->getStreamProfileList(OB_SENSOR_COLOR)
                                    ->getVideoStreamProfile(colour.width, colour.height,
                                                            colourFormat, rate);
            config->enableStream(colourProfile);
        }
        catch(const ob::Error &e) {
            std::cerr << "camera " << serial << ": " << e.what() << std::endl;
            return 1;
        }

        // One filter per camera, not one shared: it holds the calibration of the
        // pair it is aligning, and a filter is stateful.
        // Built for Raw too when the mode can change: a server started in Raw must
        // be able to produce the reference a moment later, and the filter is only
        // ever RUN when the live mode asks for it (see the callback).
        const Align filterFor = liveSwitch ? Align::Compare : align;

        std::shared_ptr<ob::Align> aligner;
        if(filterFor != Align::Raw) {
            try {
                aligner = std::make_shared<ob::Align>(
                    filterFor == Align::C2D ? OB_STREAM_DEPTH : OB_STREAM_COLOR);
                aligner->setMatchTargetResolution(filterFor != Align::D2CFov);
                // Told the target profile explicitly rather than left to infer it
                // from the first frame of the other stream: the intrinsics and
                // extrinsics are what the transform needs, and this way the very
                // first frame is aligned like all the others.
                aligner->setAlignToStreamProfile(
                    filterFor == Align::C2D ? depthProfile : colourProfile);
            }
            catch(const ob::Error &e) {
                std::cerr << "camera " << serial << ": cannot set up the "
                          << alignText(filterFor) << " transform: " << e.what()
                          << std::endl;
                return 1;
            }
        }

        // Read once, here: it depends on the profiles just enabled, and the
        // intrinsics are resolution-dependent — asking before configuring would
        // return numbers for the wrong image size.
        //
        // Built as the bytes that go on the wire, header then compressed table,
        // so the sending path has nothing left to assemble.
        auto calibration = std::make_shared<std::vector<uint8_t>>();
        try {
            OBCalibrationParam param = pipeline->getCalibrationParam(config);

            const size_t rays = static_cast<size_t>(depth.width) * depth.height * 2;
            std::vector<float> table(rays);
            // Sized from the profile rather than queried: passing a null buffer
            // to ask for the size throws. Two floats per depth pixel is what the
            // table is by definition. The SDK reports the count back in floats.
            uint32_t reported = static_cast<uint32_t>(rays);
            OBXYTables tables{};
            if(!ob::CoordinateTransformHelper::transformationInitXYTables(
                   param, OB_SENSOR_DEPTH, table.data(), &reported, &tables)) {
                throw std::runtime_error("the SDK would not build the ray table");
            }

            // The two tables come back as separate arrays; interleaved they are
            // one texture on the far end, read in a single fetch.
            std::vector<float> interleaved(rays);
            const size_t pixels = static_cast<size_t>(tables.width) * tables.height;
            for(size_t i = 0; i < pixels; i++) {
                interleaved[2 * i]     = tables.xTable[i];
                interleaved[2 * i + 1] = tables.yTable[i];
            }

            const int plainBytes = static_cast<int>(interleaved.size() * sizeof(float));
            std::vector<uint8_t> packed(
                static_cast<size_t>(LZ4_compressBound(plainBytes)));
            const int packedBytes = LZ4_compress_default(
                reinterpret_cast<const char *>(interleaved.data()),
                reinterpret_cast<char *>(packed.data()),
                plainBytes, static_cast<int>(packed.size()));
            if(packedBytes <= 0) throw std::runtime_error("could not compress the ray table");

            WireCalibrationHeader head{};
            head.depthWidth  = tables.width;
            head.depthHeight = tables.height;
            head.colourWidth  = param.intrinsics[OB_SENSOR_COLOR].width;
            head.colourHeight = param.intrinsics[OB_SENSOR_COLOR].height;
            head.colourFx = param.intrinsics[OB_SENSOR_COLOR].fx;
            head.colourFy = param.intrinsics[OB_SENSOR_COLOR].fy;
            head.colourCx = param.intrinsics[OB_SENSOR_COLOR].cx;
            head.colourCy = param.intrinsics[OB_SENSOR_COLOR].cy;
            const OBExtrinsic &d2c = param.extrinsics[OB_SENSOR_DEPTH][OB_SENSOR_COLOR];
            std::memcpy(head.rot, d2c.rot, sizeof(head.rot));
            std::memcpy(head.trans, d2c.trans, sizeof(head.trans));
            head.rayTableBytes = packedBytes;
            head.rayTablePlainBytes = plainBytes;

            calibration->resize(sizeof(head) + static_cast<size_t>(packedBytes));
            std::memcpy(calibration->data(), &head, sizeof(head));
            std::memcpy(calibration->data() + sizeof(head), packed.data(),
                        static_cast<size_t>(packedBytes));

            std::cout << "  camera " << camIndex << " calibration: ray table "
                      << tables.width << "x" << tables.height << ", "
                      << (plainBytes / 1024) << " Ko -> " << (packedBytes / 1024)
                      << " Ko" << std::endl;
        }
        catch(const std::exception &e) {
            // Not fatal: without it a receiver simply cannot align anything
            // itself, which is the state everything was in until now.
            calibration->clear();
            std::cerr << "camera " << serial << ": no calibration available: "
                      << e.what() << std::endl;
        }

        // Which sockets already have it. Erased when a viewer goes, so a
        // reconnection on the same descriptor number is sent it again.
        auto calibrationSent = std::make_shared<std::set<int>>();

        // Per-camera buffers: the callbacks run on their own threads and would
        // otherwise overwrite each other between the split and the compress.
        auto planes  = std::make_shared<std::vector<uint8_t>>();
        auto comp    = std::make_shared<std::vector<uint8_t>>();
        // A second pair, for the reference map in Compare: the first is still
        // holding the frame being sent.
        auto refPlanes = std::make_shared<std::vector<uint8_t>>();
        auto refComp   = std::make_shared<std::vector<uint8_t>>();
        auto counter = std::make_shared<uint32_t>(0);
        auto dropped = std::make_shared<std::atomic<uint32_t>>(0);

        auto warned = std::make_shared<bool>(false);
        // The size the depth comes out at is only known once a frame has been
        // through the transform, so it is announced from there rather than guessed.
        auto announced = std::make_shared<bool>(false);
        // Built only where it is used: in Raw and D2C no colour byte is ever
        // re-encoded, so there is nothing for it to do.
        auto encoder = (align == Align::C2D) ? std::make_shared<JpegEncoder>() : nullptr;
        // Held rather than handed straight to start(): the same callback is given
        // again every time this one camera is turned back on.
        std::function<void(std::shared_ptr<ob::FrameSet>)> callback =
            [&, planes, comp, counter, dropped, camIndex, aligner,
             warned, announced, encoder, calibration,
             calibrationSent, refPlanes, refComp](
                std::shared_ptr<ob::FrameSet> fs) {
            if(!g_running || fs == nullptr) return;

            // Read once for this whole frameset: it can change between framesets,
            // and a frame decided half one way and half the other would be worse
            // than either answer.
            const Align mode = static_cast<Align>(liveAlign.load());

            // Who gets this frameset: everyone connected whose link is keeping
            // up. Decided once, here, and never revisited while the messages are
            // being written — a viewer skipped halfway through would be left
            // reading a payload against the wrong header.
            std::vector<int> targets;
            for(int fd : viewers.snapshot()) {
                if(backedUp(fd, backlogLimit)) {     // behind: drop, do not queue
                    dropped->fetch_add(1);
                    continue;
                }
                targets.push_back(fd);
            }
            if(targets.empty()) return;              // nobody watching, or all behind

            // The calibration goes to a viewer once, before any frame it could
            // be needed for. Tracked per camera and per socket, so a new viewer
            // gets it without the others being sent it again.
            if(!calibration->empty()) {
                Header kh{};
                kh.magic   = kMagic;
                kh.camera  = camIndex;
                kh.kind    = 2;
                kh.payload = static_cast<uint32_t>(calibration->size());

                std::lock_guard<std::mutex> guard(sendLock);
                for(int fd : targets) {
                    if(calibrationSent->count(fd) != 0) continue;
                    if(sendAll(fd, &kh, sizeof(kh))
                       && sendAll(fd, calibration->data(), calibration->size())) {
                        calibrationSent->insert(fd);
                    }
                }
            }

            const uint64_t captured = nowUs();

            // Which frameset each half is taken from is the whole difference
            // between the modes. In Raw there is no transform at all. In D2C only
            // the depth is transformed, and the colour is still the sensor's own
            // JPEG, forwarded untouched. In C2D it is the colour that moves, so
            // both halves come from the result — and that colour is pixels now,
            // which is why it has to be encoded again below.
            //
            // process() runs here, on the thread the camera delivers on. Handing it
            // to the thread inside the filter instead was measured and changed
            // nothing: 22 frames a second in D2C either way. What limits it is the
            // transform, not this thread, and the filter also discards silently
            // when its queue is full, which cost the age its meaning.
            std::shared_ptr<ob::FrameSet> source = fs;
            std::shared_ptr<ob::FrameSet> aligned;
            if(aligner != nullptr && mode != Align::Raw) {
                try {
                    auto result = aligner->process(fs);
                    if(result == nullptr) return;
                    aligned = result->as<ob::FrameSet>();
                    source = aligned;
                }
                catch(const ob::Error &e) {
                    if(!*warned) {                   // once, not thirty times a second
                        *warned = true;
                        std::cerr << "camera " << camIndex << ": the "
                                  << alignText(mode) << " transform failed: "
                                  << e.what() << std::endl;
                    }
                    return;
                }
            }

            // In Compare the frames sent are the camera's own; the transform's
            // result travels beside them rather than replacing them.
            if(mode == Align::Compare) source = fs;

            auto depthFrame  = source->getFrame(OB_FRAME_DEPTH);
            // Depth from the result, colour from whichever frameset actually holds
            // it: in D2C the transform returns the depth alone about a third of the
            // time, and taking the colour from there cost one camera almost all of
            // its colour frames. Only C2D has a transformed colour to take.
            auto colourFrame = (mode == Align::C2D ? source : fs)->getFrame(OB_FRAME_COLOR);
            if(depthFrame == nullptr) return;

            const uint32_t stamp = (*counter)++;
            auto df = depthFrame->as<ob::DepthFrame>();
            const size_t bytes = df->getDataSize();

            if(!*announced) {
                *announced = true;
                std::cout << "  camera " << camIndex << " depth " << df->getWidth()
                          << "x" << df->getHeight() << ", " << (bytes / 1024)
                          << " Ko per frame before compression" << std::endl;
            }

            // The protocol says millimetres, and the receiver reads the 16-bit
            // values as such. The transform is not supposed to change that, but a
            // silent change of unit would look like a room the wrong size rather
            // than like an error, so it is worth one line to notice.
            if(!*warned && df->getValueScale() != 1.0f) {
                *warned = true;
                std::cerr << "camera " << camIndex << ": depth value scale is "
                          << df->getValueScale() << ", not 1 — the receiver reads "
                          << "millimetres and will be wrong by that factor."
                          << std::endl;
            }

            splitPlanes(static_cast<const uint8_t *>(df->getData()), bytes, *planes);
            comp->resize(static_cast<size_t>(LZ4_compressBound(static_cast<int>(bytes))));
            const int packed = LZ4_compress_default(
                reinterpret_cast<const char *>(planes->data()),
                reinterpret_cast<char *>(comp->data()),
                static_cast<int>(bytes), static_cast<int>(comp->size()));
            if(packed <= 0) return;

            Header hd{};
            hd.magic    = kMagic;
            hd.camera   = camIndex;
            hd.kind     = 0;
            hd.width    = static_cast<uint16_t>(df->getWidth());
            hd.height   = static_cast<uint16_t>(df->getHeight());
            hd.payload  = static_cast<uint32_t>(packed);
            hd.ageUs    = static_cast<uint32_t>(nowUs() - captured);
            hd.sequence = stamp;

            // The colour to send, and its size. In every mode but C2D these are
            // the sensor's own JPEG bytes; the receiver cannot tell the difference
            // and does not need to, which is what keeps the protocol the same
            // across all three modes.
            const uint8_t *colourData = nullptr;
            size_t colourSize = 0;
            int colourW = 0, colourH = 0;

            if(colourFrame != nullptr) {
                auto cf = colourFrame->as<ob::VideoFrame>();
                colourW = static_cast<int>(cf->getWidth());
                colourH = static_cast<int>(cf->getHeight());

                if(align == Align::C2D) {
                    if(cf->getFormat() != OB_FORMAT_RGB) {
                        if(!*warned) {
                            *warned = true;
                            std::cerr << "camera " << camIndex << ": the transform "
                                      << "returned colour in an unexpected format; "
                                      << "sending depth only." << std::endl;
                        }
                    }
                    else if(!encoder->encode(static_cast<const uint8_t *>(cf->getData()),
                                             colourW, colourH, kJpegQuality,
                                             colourData, colourSize)) {
                        colourData = nullptr;
                    }
                }
                else {
                    colourData = static_cast<const uint8_t *>(cf->getData());
                    colourSize = cf->getDataSize();
                }
            }

            // The SDK's own result, in Compare only. Same compression as the
            // depth: split planes then LZ4, so the receiver has one routine.
            const uint8_t *refData = nullptr;
            size_t refSize = 0;
            int refW = 0, refH = 0;
            if(mode == Align::Compare && aligned != nullptr) {
                auto refFrame = aligned->getFrame(OB_FRAME_DEPTH);
                if(refFrame != nullptr) {
                    auto rf = refFrame->as<ob::DepthFrame>();
                    const size_t refBytes = rf->getDataSize();
                    refW = static_cast<int>(rf->getWidth());
                    refH = static_cast<int>(rf->getHeight());
                    splitPlanes(static_cast<const uint8_t *>(rf->getData()),
                                refBytes, *refPlanes);
                    refComp->resize(static_cast<size_t>(
                        LZ4_compressBound(static_cast<int>(refBytes))));
                    const int packedRef = LZ4_compress_default(
                        reinterpret_cast<const char *>(refPlanes->data()),
                        reinterpret_cast<char *>(refComp->data()),
                        static_cast<int>(refBytes), static_cast<int>(refComp->size()));
                    if(packedRef > 0) {
                        refData = refComp->data();
                        refSize = static_cast<size_t>(packedRef);
                    }
                }
            }

            Header ch = hd;
            ch.kind    = 1;
            ch.width   = static_cast<uint16_t>(colourW);
            ch.height  = static_cast<uint16_t>(colourH);
            ch.payload = static_cast<uint32_t>(colourSize);

            // One lock for the whole frameset: two cameras must never interleave
            // their messages, since a reader takes a header then exactly its
            // payload. Sending depth and colour together under it is also what
            // makes the pair synchronised by construction rather than by
            // negotiation.
            std::vector<int> lost;
            {
                std::lock_guard<std::mutex> guard(sendLock);
                for(int fd : targets) {
                    hd.ageUs = static_cast<uint32_t>(nowUs() - captured);
                    bool ok = sendAll(fd, &hd, sizeof(hd))
                              && sendAll(fd, comp->data(), static_cast<size_t>(packed));
                    if(ok && colourData != nullptr) {
                        ch.ageUs = static_cast<uint32_t>(nowUs() - captured);
                        ok = sendAll(fd, &ch, sizeof(ch))
                             && sendAll(fd, colourData, colourSize);
                    }
                    if(ok && refData != nullptr) {
                        // Under the same lock as the pair, so all three belong to
                        // one frameset on the wire and cannot be interleaved.
                        Header rh = hd;
                        rh.kind    = 3;
                        rh.width   = static_cast<uint16_t>(refW);
                        rh.height  = static_cast<uint16_t>(refH);
                        rh.payload = static_cast<uint32_t>(refSize);
                        rh.ageUs   = static_cast<uint32_t>(nowUs() - captured);
                        ok = sendAll(fd, &rh, sizeof(rh))
                             && sendAll(fd, refData, refSize);
                    }
                    if(!ok) lost.push_back(fd);
                }
            }

            for(int fd : lost) {
                calibrationSent->erase(fd);
                viewers.drop(fd);
                std::cout << "Viewer gone (" << viewers.count() << " watching)."
                          << std::endl;
            }
        };

        Cam cam;
        cam.serial   = serial;
        cam.camIndex = camIndex;
        cam.pipeline = pipeline;
        cam.config   = config;
        cam.callback = callback;
        if(wanted) {
            pipeline->start(config, callback);
            cam.running = true;
        }
        cams.push_back(cam);
        dropCounters.push_back(dropped);
        std::cout << "  camera " << camIndex << " : " << serial
                  << (wanted ? "" : "  (built, not streaming)") << std::endl;
    }

    size_t streaming = 0;
    for(const auto &c : cams) if(c.running) streaming++;

    // With a composition file there is nothing wrong with starting empty — it can
    // be filled a moment later. Without one, an empty selection is a mistake worth
    // failing on rather than holding the port and sending nothing.
    if(streaming == 0 && mapFile.empty()) {
        std::cerr << "nothing to serve: every camera is excluded by --map"
                  << std::endl;
        // Same teardown as the no-camera path above, and for the same reason.
        ::shutdown(listenFd, SHUT_RDWR);
        ::close(listenFd);
        accepter.detach();
        return 1;
    }

    std::cout << "Serving " << streaming << " camera(s) on port " << port
              << " — colour " << colour.width << "x" << colour.height
              << " JPEG, depth " << depth.width << "x" << depth.height
              << " 16-bit lossless, " << rate << " fps, alignment " << alignName
              << std::endl;

    uint32_t reported = 0;
    int ticks = 0;
    while(g_running) {
        std::this_thread::sleep_for(std::chrono::seconds(1));
        ticks++;

        // Bring the running set in line with the file. A camera leaving it is
        // stopped and the others carry on: that is the whole reason this exists
        // rather than a restart.
        if(!mapFile.empty()) {
            const Composition now = readComposition(mapFile);
            const std::map<std::string, uint16_t> &wantedNow = now.indices;

            // The mode first, and independently of the camera list: it costs
            // nothing but a flag, and the next frameset picks it up.
            if(now.hasAlign && liveSwitch) {
                const int wantMode = static_cast<int>(now.align);
                if(wantMode != liveAlign.load()
                   && (now.align == Align::Raw || now.align == Align::Compare)) {
                    liveAlign.store(wantMode);
                    std::cout << "alignment now " << alignText(now.align)
                              << " (no restart)." << std::endl;
                }
            }

            // An unreadable or empty file is far more likely to be a half-written
            // one than a real request to stop everything, and acting on it would be
            // a spectacular way to lose a take. So it is ignored until it reads.
            if(!wantedNow.empty()) {
                for(auto &cam : cams) {
                    const bool want = wantedNow.find(cam.serial) != wantedNow.end();
                    if(want == cam.running) continue;
                    try {
                        if(want) {
                            cam.pipeline->start(cam.config, cam.callback);
                            cam.running = true;
                            std::cout << "camera " << cam.camIndex << " (" << cam.serial
                                      << ") streaming again." << std::endl;
                        }
                        else {
                            cam.pipeline->stop();
                            cam.running = false;
                            std::cout << "camera " << cam.camIndex << " (" << cam.serial
                                      << ") stopped, it left the composition."
                                      << std::endl;
                        }
                    }
                    catch(const ob::Error &e) {
                        // Leaves the flag alone so the next pass tries again, and
                        // says which camera: one sensor refusing to stop must not
                        // take the others down with it.
                        std::cerr << "camera " << cam.camIndex << ": "
                                  << (want ? "start" : "stop") << " failed: "
                                  << e.what() << std::endl;
                    }
                }
            }
        }

        if(ticks % 5 != 0) continue;
        uint32_t total = 0;
        for(auto &d : dropCounters) total += d->load();
        if(total != reported) {
            std::cout << "  dropped " << (total - reported)
                      << " frame(s): the link is behind, or the transform is"
                      << std::endl;
            reported = total;
        }
    }

    // Stopping the pipelines can block in the SDK while it releases its EGL
    // contexts, so shut the socket first: a viewer sees the end immediately
    // instead of waiting on a teardown it has no interest in.
    ::shutdown(listenFd, SHUT_RDWR);
    ::close(listenFd);
    accepter.detach();
    viewers.closeAll();

    for(auto &cam : cams) if(cam.running) cam.pipeline->stop();
    std::cout << "Stopped." << std::endl;
    return 0;
}