// SICK nanoScan3 / microScan3 — binary safety-data UDP packets, with
// application-layer "MS3 " fragment reassembly.
#include "sick_safety_driver.hpp"
#include "plugin_helpers.hpp"

#include <cerrno>
#include <cmath>
#include <cstring>
#include <limits>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <unistd.h>

namespace xlidar {

namespace {
constexpr size_t kNanoRecvBufSize = 65536;
// nanoScan3 DerivedValues store angles as int32 in 1/4194304 degree.
constexpr double kNanoAngleResolution = 4194304.0;
} // namespace

SickSafetyDriver::SickSafetyDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
                                   bool inverted)
    : cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
      inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}

SickSafetyDriver::~SickSafetyDriver() { close(); }

ErrorCode SickSafetyDriver::open() {
    if (is_open()) return set_error(ErrorCode::AlreadyOpen);

    sockaddr_in addr{};
    addr.sin_family = AF_INET;
    addr.sin_port   = htons(port_);
    if (ip_ == "0.0.0.0" || ip_.empty()) {
        addr.sin_addr.s_addr = INADDR_ANY;
    } else if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1) {
        return set_error(ErrorCode::InvalidAddress);
    }

    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);

    // No SO_REUSEADDR: UDP has no TIME_WAIT, and on Linux it would let two
    // sockets bind the same port, hiding PortInUse from the second app.
    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
        int err = errno;
        ::close(sock_fd_);
        sock_fd_ = -1;
        return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
                                                              : ErrorCode::BindFailed);
    }
    latest_diag_ = Diagnostics{};
    return set_error(ErrorCode::Ok);
}

void SickSafetyDriver::close() {
    if (sock_fd_ >= 0) {
        ::close(sock_fd_);
        sock_fd_ = -1;
    }
}

int SickSafetyDriver::recv_datagram(int timeout_ms) {
    if (!is_open()) { set_error(ErrorCode::NotOpen); return -1; }

    if (timeout_ms > 0) {
        fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
        timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
        int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
        if (r <= 0) {
            set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
            return -1;
        }
    }

    ssize_t n = ::recv(sock_fd_, recv_buf_.data(), recv_buf_.size(), 0);
    if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return -1; }
    return static_cast<int>(n);
}

// A scan is split across datagrams at the application layer. Each starts with
// a 24-byte fragment header: "MS3 " @0, u32 totalLength @8, u32 scanNumber @12,
// u32 fragmentOffset @16. Reassemble until totalLength bytes; a lost fragment
// drops that scan and we resync on the next scanNumber.
bool SickSafetyDriver::recv_scan(ScanResult& out, int timeout_ms) {
    std::vector<uint8_t> tele;
    std::vector<uint8_t> have;  // per-byte coverage so duplicate fragments don't count twice
    uint32_t cur_scan = 0, total = 0, got = 0;
    bool assembling = false;

    for (;;) {
        int n = recv_datagram(timeout_ms);
        if (n < 0) return false;
        const uint8_t* d = recv_buf_.data();

        if (n < 24 || std::memcmp(d, "MS3 ", 4) != 0) {
            if (parse_packet(d, n, out)) { set_error(ErrorCode::Ok); return true; }
            continue;
        }

        uint32_t tl    = le32(d + 8);
        uint32_t scan  = le32(d + 12);
        uint32_t foff  = le32(d + 16);
        const uint8_t* pl = d + 24;
        uint32_t pl_len   = static_cast<uint32_t>(n) - 24;
        if (tl == 0 || tl > kNanoRecvBufSize) continue;

        if (!assembling || scan != cur_scan || tl != total) {
            cur_scan = scan; total = tl; got = 0;
            tele.assign(total, 0);
            have.assign(total, 0);
            assembling = true;
        }

        if (static_cast<uint64_t>(foff) + pl_len <= total) {
            std::memcpy(tele.data() + foff, pl, pl_len);
            for (uint32_t b = 0; b < pl_len; ++b)
                if (!have[foff + b]) { have[foff + b] = 1; ++got; }
        }

        if (got >= total) {
            assembling = false;
            if (parse_packet(tele.data(), static_cast<int>(total), out)) {
                set_error(ErrorCode::Ok);
                return true;
            }
        }
    }
}

bool SickSafetyDriver::spin_once() {
    int n = recv_datagram(0);
    if (n < 0) return false;

    ScanResult result;
    if (!parse_packet(recv_buf_.data(), n, result)) return true;
    if (cb_) cb_(result);
    return true;
}

// SICK safety-scanner data packet (LE), layout ported from sick_safetyscanners:
// DataHeader offset table at fixed offsets (derivedValues @36, measurementData
// @40); DerivedValues holds multiplicationFactor/startAngle/resolution;
// MeasurementData is u32 numBeams then 4 B/beam (u16 dist, u8 reflect, u8 status).
bool SickSafetyDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
    if (len < 52) return false;

    uint16_t gss_off = le16(buf + 32);  // General System State block
    uint16_t gss_size = le16(buf + 34);
    uint16_t dv_off  = le16(buf + 36);
    uint16_t dv_size = le16(buf + 38);
    uint16_t md_off  = le16(buf + 40);
    uint16_t md_size = le16(buf + 42);

    if (dv_off == 0 || dv_size == 0 || md_off == 0 || md_size == 0) return false;
    if (static_cast<int>(dv_off) + 20 > len) return false;
    if (static_cast<int>(md_off) + 4  > len) return false;

    const uint8_t* dv = buf + dv_off;
    uint16_t mult_factor = le16(dv + 0);
    int32_t  start_raw   = le_i32(dv + 8);
    int32_t  res_raw     = le_i32(dv + 12);
    if (mult_factor == 0) mult_factor = 1;

    double start_deg = static_cast<double>(start_raw) / kNanoAngleResolution;
    double res_deg   = static_cast<double>(res_raw)   / kNanoAngleResolution;

    const uint8_t* md   = buf + md_off;
    uint32_t num_beams  = le32(md + 0);
    if (num_beams == 0 || num_beams > 2751) return false;  // 2751 = sensor max
    if (static_cast<int64_t>(md_off) + 4 + static_cast<int64_t>(num_beams) * 4 > len)
        return false;

    LaserScan& scan = out.scan;
    scan.ranges.assign(num_beams, 0.f);
    scan.intensities.assign(num_beams, 0.f);

    for (uint32_t i = 0; i < num_beams; ++i) {
        const uint8_t* p   = md + 4 + i * 4;
        uint16_t distance  = le16(p + 0);
        uint8_t  reflect   = le_u8(p + 2);
        uint8_t  status    = le_u8(p + 3);
        bool     valid     = (status & 0x01) != 0;
        bool     infinite  = (status & 0x02) != 0;

        if (!valid || infinite) {
            scan.ranges[i] = std::numeric_limits<float>::infinity();
        } else {
            scan.ranges[i] = static_cast<float>(distance) *
                             static_cast<float>(mult_factor) * 1e-3f;  // mm -> m
        }
        scan.intensities[i] = static_cast<float>(reflect);
    }

    scan.angle_min       = (static_cast<float>(start_deg) + cfg_.angle_offset_deg) * kDeg2Rad;
    scan.angle_increment = static_cast<float>(res_deg   * kDeg2Rad);
    scan.angle_max       = scan.angle_min +
        scan.angle_increment * static_cast<float>(num_beams - 1);
    scan.time_increment  = 0.f;
    scan.scan_time       = 0.f;
    scan.range_min       = cfg_.range_min_m;
    scan.range_max       = cfg_.range_max_m;
    // Raw device time from the DataHeader — an opaque tag, not ms since power-on.
    scan.timestamp_ms    = le32(buf + 28);

    finalize_scan(scan, cfg_, inverted_);

    ExtraInfo& info = out.info;
    info = ExtraInfo{};
    info.detected_model = cfg_.name;
    // Byte 0 holds the run/standby/contamination/manipulation flags
    // (kNanoState*); the block is absent when not configured in the sensor.
    if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
        info.nano_general_state = buf[gss_off];

    latest_diag_ = decode_diagnostics(info);
    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
    mark_scan_decoded();

    return true;
}

// ── plugin registration ─────────────────────────────────────────────────────

namespace {

const DriverInfo kDriverInfo = [] {
    DriverInfo info;
    info.vendor      = "SICK";
    info.model       = "nanoScan3/microScan3";
    info.driver_id   = "sick_nanoscan3_driver";
    info.description = "SICK safety laser scanners (nanoScan3/microScan3 family) "
                       "— passive receiver of the binary safety-data UDP output; "
                       "the sensor's UDP target must be configured in SICK Safety "
                       "Designer. Default local port 6060. Not verified on real "
                       "hardware.";
    info.transport   = Transport::Udp;
    info.supported_models = {"SICK-nanoScan3"};
    return info;
}();

} // namespace

DriverInfo SickSafetyDriver::get_driver_info() const { return kDriverInfo; }

} // namespace xlidar

XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
    *out = xlidar::kDriverInfo;
}

XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
create_driver_instance(const xlidar::DeviceConfig* cfg) {
    using namespace xlidar;
    if (!transport_supported(kDriverInfo, *cfg))
        return new InvalidConfigDriver(kDriverInfo,
            std::string("unsupported transport '") + to_string(*cfg->transport) + "'");
    const uint16_t port = cfg->port ? cfg->port : 6060;
    return new SickSafetyDriver(apply_device_config(MODEL_SICK_NANOSCAN3, *cfg),
                                cfg->ip, port, cfg->inverted);
}