// SICK TiM 5xx/7xx — SOPAS/CoLa-A ASCII telegrams over TCP ("sSN/sRA
// LMDscandata" parsing).
#include "sick_code_driver.hpp"
#include "plugin_helpers.hpp"

#include <cctype>
#include <cerrno>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/select.h>
#include <sys/socket.h>
#include <unistd.h>

namespace xlidar {

namespace {
constexpr char  kStx              = 0x02;
constexpr char  kEtx              = 0x03;
constexpr int   kConnectTimeoutMs = 2000;

uint32_t hex_to_u32(const std::string& tok) {
    return static_cast<uint32_t>(std::strtoul(tok.c_str(), nullptr, 16));
}
int32_t hex_to_i32(const std::string& tok) {
    // SICK encodes signed fields as plain hex of the 2's-complement bits.
    return static_cast<int32_t>(hex_to_u32(tok));
}

std::vector<std::string> tokenize(const std::string& s) {
    std::vector<std::string> out;
    size_t i = 0, n = s.size();
    while (i < n) {
        while (i < n && std::isspace(static_cast<unsigned char>(s[i]))) ++i;
        size_t start = i;
        while (i < n && !std::isspace(static_cast<unsigned char>(s[i]))) ++i;
        if (i > start) out.push_back(s.substr(start, i - start));
    }
    return out;
}
} // namespace

SickCodeDriver::SickCodeDriver(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) {}

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

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

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

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

    ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
    if (conn_err != ErrorCode::Ok) {
        ::close(sock_fd_);
        sock_fd_ = -1;
        return set_error(conn_err);
    }

    recv_buf_.clear();
    latest_diag_ = Diagnostics{};

    // Device is passive until told to stream.
    if (!send_telegram("sEN LMDscandata 1")) {
        close();
        return set_error(ErrorCode::HandshakeFailed);
    }
    return set_error(ErrorCode::Ok);
}

void SickCodeDriver::close() {
    if (sock_fd_ >= 0) {
        send_telegram("sEN LMDscandata 0");  // best-effort
        ::close(sock_fd_);
        sock_fd_ = -1;
    }
}

bool SickCodeDriver::send_telegram(const std::string& body) {
    if (sock_fd_ < 0) return false;
    std::string framed;
    framed.reserve(body.size() + 2);
    framed.push_back(kStx);
    framed += body;
    framed.push_back(kEtx);

    size_t sent = 0;
    while (sent < framed.size()) {
        ssize_t n = ::send(sock_fd_, framed.data() + sent, framed.size() - sent, 0);
        if (n <= 0) return false;
        sent += static_cast<size_t>(n);
    }
    return true;
}

// CoLa-A has no length prefix, so ETX is the only frame boundary; recv_buf_
// carries leftover bytes across calls.
bool SickCodeDriver::read_telegram(std::string& out, int timeout_ms) {
    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }

    for (;;) {
        size_t etx_pos = recv_buf_.find(kEtx);
        if (etx_pos != std::string::npos) {
            size_t stx_pos = recv_buf_.find(kStx);
            if (stx_pos == std::string::npos || stx_pos > etx_pos) {
                recv_buf_.erase(0, etx_pos + 1);
                continue;
            }
            out = recv_buf_.substr(stx_pos + 1, etx_pos - stx_pos - 1);
            recv_buf_.erase(0, etx_pos + 1);
            return true;
        }

        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 false;
            }
        }

        char buf[4096];
        ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
        if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
        recv_buf_.append(buf, static_cast<size_t>(n));
    }
}

bool SickCodeDriver::recv_scan(ScanResult& out, int timeout_ms) {
    for (;;) {
        std::string telegram;
        if (!read_telegram(telegram, timeout_ms)) return false;
        if (parse_lmdscandata(telegram, out)) { set_error(ErrorCode::Ok); return true; }
        // Non-scan telegram (e.g. an ack) — keep waiting.
    }
}

bool SickCodeDriver::spin_once() {
    std::string telegram;
    if (!read_telegram(telegram, 0)) return false;

    ScanResult result;
    if (!parse_lmdscandata(telegram, result)) return true;
    if (cb_) cb_(result);
    return true;
}

// CoLa-A "sSN/sRA LMDscandata": space-separated ASCII hex tokens, field order
// per SICK's Telegram Listing. "DIST1" → ranges, "RSSI1" → intensities.
bool SickCodeDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out) {
    std::vector<std::string> tok = tokenize(telegram);
    if (tok.size() < 20) return false;
    if (tok[0] != "sSN" && tok[0] != "sRA") return false;
    if (tok[1] != "LMDscandata") return false;

    size_t i = 2;
    auto next = [&]() -> std::string { return (i < tok.size()) ? tok[i++] : std::string(); };

    hex_to_u32(next());  // VersionNumber
    hex_to_u32(next());  // DeviceNumber
    hex_to_u32(next());  // SerialNumber
    uint32_t status0              = hex_to_u32(next());
    uint32_t status1              = hex_to_u32(next());
    hex_to_u32(next());  // TelegramCounter
    hex_to_u32(next());  // ScanCounter
    hex_to_u32(next());  // TimeSinceStartup
    uint32_t time_of_transmission = hex_to_u32(next());
    uint32_t in0  = hex_to_u32(next());
    uint32_t in1  = hex_to_u32(next());
    uint32_t out0 = hex_to_u32(next());
    uint32_t out1 = hex_to_u32(next());
    next();  // Reserved
    uint32_t scanning_frequency = hex_to_u32(next());
    hex_to_u32(next());  // MeasurementFrequency

    uint32_t num_encoders = hex_to_u32(next());
    for (uint32_t e = 0; e < num_encoders; ++e) {
        next();  // EncoderPosition
        next();  // EncoderSpeed
    }

    LaserScan& scan = out.scan;
    scan.ranges.clear();
    scan.intensities.clear();
    float angle_min_deg = 0.f, angle_inc_deg = 0.f;
    bool  got_dist       = false;

    // 16-bit and 8-bit channel blocks share the same ASCII layout.
    auto parse_channel_block = [&]() {
        std::string content    = next();
        uint32_t    scale_bits = hex_to_u32(next());
        hex_to_u32(next());                            // ScalingOffset
        int32_t     start_angle = hex_to_i32(next());  // 1/10000 deg
        int32_t     step_width  = hex_to_i32(next());  // 1/10000 deg
        uint32_t    num_data    = hex_to_u32(next());

        float scale = bits_to_float(scale_bits);
        if (scale == 0.f) scale = 1.f;

        bool is_dist = content.rfind("DIST", 0) == 0;
        bool is_rssi = content.rfind("RSSI", 0) == 0;

        if (is_dist) {
            angle_min_deg = static_cast<float>(start_angle) * 0.0001f + cfg_.angle_offset_deg;
            angle_inc_deg = static_cast<float>(step_width)  * 0.0001f;
            scan.ranges.assign(num_data, 0.f);
        } else if (is_rssi && scan.intensities.empty()) {
            scan.intensities.assign(num_data, 0.f);
        }

        for (uint32_t d = 0; d < num_data; ++d) {
            uint32_t raw = hex_to_u32(next());
            if (is_dist) {
                scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f;  // mm -> m
                got_dist       = true;
            } else if (is_rssi && d < scan.intensities.size()) {
                // Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it).
                float v = static_cast<float>(raw) * scale;
                scan.intensities[d] = v > 255.f ? 255.f : v;
            }
        }
    };

    uint32_t num_16bit_channels = hex_to_u32(next());
    for (uint32_t c = 0; c < num_16bit_channels; ++c) parse_channel_block();

    uint32_t num_8bit_channels = hex_to_u32(next());
    for (uint32_t c = 0; c < num_8bit_channels; ++c) parse_channel_block();

    if (!got_dist || scan.ranges.empty()) return false;

    scan.timestamp_ms    = time_of_transmission;
    scan.angle_min       = angle_min_deg * kDeg2Rad;
    scan.angle_increment = angle_inc_deg * kDeg2Rad;
    scan.angle_max        = scan.angle_min +
        scan.angle_increment * static_cast<float>(scan.ranges.size() - 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;
    if (scan.intensities.size() != scan.ranges.size())
        scan.intensities.assign(scan.ranges.size(), 0.f);

    finalize_scan(scan, cfg_, inverted_);

    ExtraInfo& info = out.info;
    info = ExtraInfo{};
    info.detected_model     = cfg_.name;
    info.sick_device_status = static_cast<uint16_t>(((status0 & 0xFF) << 8) | (status1 & 0xFF));
    info.status_flags       = (status0 << 8) | status1;
    info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
    info.input_status        = static_cast<uint16_t>((in0 << 8) | in1);
    info.output_status        = static_cast<uint16_t>((out0 << 8) | out1);

    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       = "TiM5xx/TiM7xx";
    info.driver_id   = "sick_tim_driver";
    info.description = "SICK TiM 2D lidars (TiM551/561/571/781, ...) over "
                       "SOPAS/CoLa-A ASCII telegrams on TCP. open() starts the "
                       "LMDscandata stream. Default port 2111. Verified on a "
                       "real TiM781S.";
    info.transport   = Transport::Tcp;
    info.supported_models = {"SICK-TIM5xx", "SICK-TIM571", "SICK-TIM7xx"};
    return info;
}();

const ModelConfig* model_by_name(const std::string& name) {
    static constexpr const ModelConfig* kModels[] = {
        &MODEL_SICK_TIM5XX, &MODEL_SICK_TIM571, &MODEL_SICK_TIM7XX,
    };
    for (const ModelConfig* m : kModels)
        if (name == m->name) return m;
    return nullptr;
}

} // namespace

DriverInfo SickCodeDriver::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;
    const ModelConfig* preset = model_by_name(cfg->model);
    if (!preset) preset = &MODEL_SICK_TIM571;  // brand default
    const uint16_t port = cfg->port ? cfg->port : 2111;
    return new SickCodeDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
}