DriverLIdar/plugins/common/plugin_helpers.hpp

// Internal helpers shared by the plugin TUs — not part of the public API.
// Header-only on purpose: every plugin .so carries its own copy, so plugins
// never link against each other or against liblidar_manager.
#pragma once
#include "lidar_interface.hpp"

#include <algorithm>
#include <cerrno>
#include <cmath>
#include <cstdint>
#include <cstring>
#include <fcntl.h>
#include <limits>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <sys/socket.h>

namespace xlidar {

inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;

// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
    const float new_min  = min_deg * kDeg2Rad;
    const float new_max  = max_deg * kDeg2Rad;
    const float old_span = scan.angle_max - scan.angle_min;
    if (old_span > 0.f)
        scan.angle_increment *= (new_max - new_min) / old_span;
    scan.angle_min = new_min;
    scan.angle_max = new_max;
}

// Mirror a finished scan for a unit mounted upside-down: reverse the point
// order and negate the angular window. Apply before remap_scan_window().
inline void invert_scan(LaserScan& scan) {
    std::reverse(scan.ranges.begin(), scan.ranges.end());
    std::reverse(scan.intensities.begin(), scan.intensities.end());
    const float new_min = -scan.angle_max;
    scan.angle_max = -scan.angle_min;
    scan.angle_min = new_min;
}

// Valid FOV window (DeviceConfig::angle_min/max_deg): points whose signed
// angle falls outside [min_deg, max_deg] become NaN; the scan geometry is
// unchanged. Apply after invert_scan(), before remap_scan_window() (it needs
// the real angles).
inline void apply_fov_window(LaserScan& scan, float min_deg, float max_deg) {
    const float min_rad = min_deg * kDeg2Rad;
    const float max_rad = max_deg * kDeg2Rad;
    constexpr float kPi = 3.14159265358979323846f;
    for (size_t i = 0; i < scan.ranges.size(); ++i) {
        // Normalize into (-pi, pi]: OLEI scans unwrap continuously and may
        // exceed the seam.
        float a = scan.angle_min + static_cast<float>(i) * scan.angle_increment;
        a = std::fmod(a, 2.f * kPi);
        if (a >  kPi) a -= 2.f * kPi;
        if (a < -kPi) a += 2.f * kPi;
        if (a < min_rad || a > max_rad)
            scan.ranges[i] = std::numeric_limits<float>::quiet_NaN();
    }
}

// Apply the generic DeviceConfig windows/overrides onto a model preset —
// every plugin's create_driver_instance() funnels through this.
inline ModelConfig apply_device_config(const ModelConfig& preset, const DeviceConfig& cfg) {
    ModelConfig mc = preset;
    if (cfg.range_min_m > 0.f) mc.range_min_m = cfg.range_min_m;
    if (cfg.range_max_m > 0.f) mc.range_max_m = cfg.range_max_m;
    if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
        mc.fov_filter  = true;
        mc.fov_min_deg = cfg.angle_min_deg;
        mc.fov_max_deg = cfg.angle_max_deg;
    }
    if (cfg.remap_angle_min_deg > -360.f || cfg.remap_angle_max_deg < 360.f) {
        mc.remap_angles  = true;
        mc.out_angle_min = cfg.remap_angle_min_deg;
        mc.out_angle_max = cfg.remap_angle_max_deg;
    }
    return mc;
}

// Standard finalize sequence shared by the drivers; call once per completed
// scan, after ranges/intensities/angles are filled in device order.
inline void finalize_scan(LaserScan& scan, const ModelConfig& cfg, bool inverted) {
    if (inverted)
        invert_scan(scan);
    if (cfg.fov_filter)
        apply_fov_window(scan, cfg.fov_min_deg, cfg.fov_max_deg);
    if (cfg.remap_angles)
        remap_scan_window(scan, cfg.out_angle_min, cfg.out_angle_max);
}

// Little-endian readers (bounds are the caller's responsibility).
inline uint8_t  le_u8 (const uint8_t* p) { return p[0]; }
inline uint16_t le16(const uint8_t* p) {
    return static_cast<uint16_t>(p[0] | (p[1] << 8));
}
inline uint32_t le32(const uint8_t* p) {
    return static_cast<uint32_t>(p[0])
         | (static_cast<uint32_t>(p[1]) <<  8)
         | (static_cast<uint32_t>(p[2]) << 16)
         | (static_cast<uint32_t>(p[3]) << 24);
}
inline int32_t  le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }

inline float bits_to_float(uint32_t bits) {
    float f;
    std::memcpy(&f, &bits, sizeof(f));
    return f;
}

// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
// way. The caller owns the fd and closes it on failure.
inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
    int flags = ::fcntl(fd, F_GETFL, 0);
    ::fcntl(fd, F_SETFL, flags | O_NONBLOCK);

    ErrorCode conn_err = ErrorCode::Ok;
    int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
    if (rc < 0 && errno != EINPROGRESS) {
        conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
                                           : ErrorCode::ConnectionFailed;
    } else if (rc < 0) {
        fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
        rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
        if (rc == 0) {
            conn_err = ErrorCode::Timeout;
        } else if (rc < 0) {
            conn_err = ErrorCode::ConnectionFailed;
        } else {
            int err = 0; socklen_t errlen = sizeof(err);
            ::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
            if (err != 0)
                conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
                                                 : ErrorCode::ConnectionFailed;
        }
    }
    ::fcntl(fd, F_SETFL, flags);

    if (conn_err == ErrorCode::Ok) {
        int nodelay = 1;
        ::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
    }
    return conn_err;
}

} // namespace xlidar