// ESPE LGA60 — "HISN" range frames + "WSimu" area frames over TCP/UDP.
#include "espe_driver.hpp"
#include "plugin_helpers.hpp"

#include <algorithm>
#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 {
// "RAuto" + fixed tail — puts the device into continuous measurement output.
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};

constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
constexpr char kAreaMagic[5]  = {'W', 'S', 'i', 'm', 'u'};

constexpr size_t   kRangeHeaderSize = 16;    // magic + 6 big-endian u16 fields
constexpr size_t   kAreaFrameSize   = 13;    // magic + 4 status bytes + err u16 + crc u16
constexpr uint16_t kMaxDistanceMm   = 50000; // wire sentinel: beyond = no return
constexpr uint16_t kMaxIntensity    = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
constexpr int      kConnectTimeoutMs = 2000;

// The head measures over a fixed window of every turn — from 20° to 340° in
// device angles, 0° at the rear — and is blind over the remaining 40°. Both
// the revolution's size and every packet's position are anchored to that
// window (the vendor ROS driver hard-codes the same two numbers). Consistent
// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
// applied.
constexpr uint16_t kSweepStartDeg = 20;
constexpr uint16_t kSweepEndDeg   = 340;
constexpr float    kSweepSpanDeg  = static_cast<float>(kSweepEndDeg - kSweepStartDeg);
constexpr float    kFullTurnDeg   = 360.f;

// Rotation periods above this are stalls/reconnects, not a spin rate (the
// device runs at 10 or 20 Hz): don't publish timing derived from them.
constexpr float kMaxRevPeriodS = 1.f;

// A point no packet ever delivered — "invalid", distinct from the infinity
// that means the device looked and got no return.
const float kMissingPoint = std::numeric_limits<float>::quiet_NaN();

uint16_t be16(const uint8_t* p) {
    return static_cast<uint16_t>((p[0] << 8) | p[1]);
}
} // namespace

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

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

ErrorCode EspeDriver::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, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
    if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);

    ErrorCode conn_err = ErrorCode::Ok;
    if (use_udp_) {
        // connect() on UDP just fixes the peer; replies come to our port.
        if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
            conn_err = ErrorCode::ConnectionFailed;
    } else {
        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();
    pending_ranges_.clear();
    pending_intensities_.clear();
    angle_inc_deg_  = 0.f;
    points_total_   = 0;
    have_last_rev_  = false;
    scan_ready_     = false;
    espe_error_status_.reset();
    latest_diag_  = Diagnostics{};

    // Device is passive until told to stream.
    ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
    if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
        close();
        return set_error(ErrorCode::HandshakeFailed);
    }
    return set_error(ErrorCode::Ok);
}

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

bool EspeDriver::fill_buffer(int timeout_ms) {
    if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }

    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));
    return true;
}

// Consume complete frames from recv_buf_; returns true once a full revolution
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
bool EspeDriver::parse_buffer() {
    for (;;) {
        size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
        size_t area_pos  = recv_buf_.find(kAreaMagic,  0, sizeof(kAreaMagic));
        size_t pos = std::min(range_pos, area_pos);
        if (pos == std::string::npos) {
            // No magic in sight: keep only a possible partial magic at the tail.
            if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
                recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
            return scan_ready_;
        }
        if (pos > 0) recv_buf_.erase(0, pos);

        const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());

        if (area_pos < range_pos) {
            if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
            // Zone/obstacle frame — only sent when the host polls areas, but
            // it carries the device fault word, so latch it if it appears.
            // Byte order unverified on hardware: the protocol is mixed-endian
            // (header fields big-endian, point payload little-endian) and no
            // spec covers this field; little-endian assumed like the payload.
            espe_error_status_ = le16(d + 9);
            recv_buf_.erase(0, kAreaFrameSize);
            continue;
        }

        if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
        uint16_t data_size    = be16(d + 8);
        uint16_t measure_size = be16(d + 12);
        if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
            recv_buf_.erase(0, sizeof(kRangeMagic));  // bogus header — resync
            continue;
        }
        // Both counters describe this packet; the clamp is the vendor's.
        if (data_size > measure_size) data_size = measure_size;

        size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
        if (recv_buf_.size() < frame_size) return scan_ready_;

        handle_range_frame(d, data_size);
        recv_buf_.erase(0, frame_size);
        // Stop as soon as a revolution completes — draining further frames
        // could finish a second revolution and overwrite ready_result_ before
        // the caller consumes it. Leftover bytes wait for the next call.
        if (scan_ready_) return true;
    }
}

// Latch the angular step and size the revolution around it. Kept stable once
// latched: the wire angles are whole degrees, so a step re-derived from a
// mid-sweep packet jitters, and re-sizing would drop the sweep in flight.
void EspeDriver::set_resolution(float inc_deg) {
    if (!(inc_deg > 0.f)) return;
    const long total = std::lround(kSweepSpanDeg / inc_deg);
    if (total < 2 || total > static_cast<long>(kMaxPointsPerRev)) return;  // implausible step
    if (angle_inc_deg_ > 0.f && static_cast<uint32_t>(total) == points_total_) return;

    angle_inc_deg_ = inc_deg;
    points_total_  = static_cast<uint32_t>(total);
    begin_revolution();
}

void EspeDriver::begin_revolution() {
    if (points_total_ == 0) return;
    pending_ranges_.assign(points_total_, kMissingPoint);
    pending_intensities_.assign(points_total_, 0.f);
}

// Range frame: "HISN", then big-endian u16 start_angle, end_angle (the
// angular window THIS packet covers, whole degrees), data_size (points in
// this packet's payload), data_position and measure_size (the vendor's
// "position"/"count" of the current packet's points), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
//
// A packet is a slice of the sweep, not a revolution: the device splits every
// 20°→340° sweep into several of them, the first opening at 20° and the last
// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
// — reading measure_size as the revolution size (as this driver first did)
// emits one scan per packet, each covering only that packet's few degrees.
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
    const uint16_t start_angle   = be16(frame + 4);
    const uint16_t end_angle     = be16(frame + 6);
    const uint16_t data_position = be16(frame + 10);
    const uint16_t measure_size  = be16(frame + 12);
    // frame + 14 is a 16-bit device counter the vendor header marks as "time
    // flag (not enabled)"; see finish_scan() for why it is not a timestamp.

    // Step = this packet's angular span / its point count. Taken from the
    // packet that opens a sweep — the one the vendor driver trusts — or from
    // whatever arrives first while nothing is latched yet.
    if (start_angle == kSweepStartDeg || angle_inc_deg_ <= 0.f) {
        const float span = static_cast<float>(end_angle) - static_cast<float>(start_angle);
        if (span > 0.f && measure_size > 0)
            set_resolution(span / static_cast<float>(measure_size));
    }
    if (angle_inc_deg_ <= 0.f) return;  // step still unknown — nowhere to put the points

    // First packet of a sweep: drop anything a lost closing packet left behind.
    if (start_angle == kSweepStartDeg && data_position <= data_size) begin_revolution();

    // Index of this packet's first point within the sweep, verbatim from the
    // vendor driver: the angular offset from 20°, plus what the header's own
    // counters carry. Exactly one of the two terms moves, whichever way the
    // firmware numbers its packets — either start_angle walks the sweep while
    // data_position stays at this packet's own count, or start_angle stays at
    // 20° while data_position accumulates — so the sum is the packet's true
    // start index in both cases.
    const int32_t begin =
        static_cast<int32_t>(std::lround(
            (static_cast<float>(start_angle) - static_cast<float>(kSweepStartDeg)) / angle_inc_deg_))
        + static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);

    // Integer wire angles make 320°/step land a point or two short of what the
    // device actually streams; grow rather than clip the tail (the vendor
    // driver does the same).
    const size_t needed = static_cast<size_t>(begin > 0 ? begin : 0) + data_size;
    if (needed > pending_ranges_.size() && needed <= kMaxPointsPerRev) {
        pending_ranges_.resize(needed, kMissingPoint);
        pending_intensities_.resize(needed, 0.f);
        points_total_ = static_cast<uint32_t>(needed);
    }

    const uint8_t* p = frame + kRangeHeaderSize;
    for (uint16_t i = 0; i < data_size; ++i, p += 4) {
        const int32_t idx = begin + i;
        if (idx < 0 || idx >= static_cast<int32_t>(pending_ranges_.size())) continue;
        const uint16_t dist  = le16(p + 0);
        const uint16_t inten = le16(p + 2);
        pending_ranges_[idx] = (dist > kMaxDistanceMm)
            ? std::numeric_limits<float>::infinity()
            : static_cast<float>(dist) * 1e-3f;  // mm -> m
        // Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
        pending_intensities_[idx] =
            static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
            * (255.f / kMaxIntensity);
    }

    // The packet that closes the sweep at 340°, with its point counter full,
    // ends the revolution — the vendor driver's condition unchanged. It holds
    // whichever way the firmware numbers packets: per-packet counters make
    // data_position == measure_size true on every packet (so the 340° edge
    // decides), cumulative ones make it true only on the sweep's last packet.
    if (end_angle == kSweepEndDeg && data_position == measure_size) finish_scan();
}

void EspeDriver::finish_scan() {
    if (pending_ranges_.size() < 2 || angle_inc_deg_ <= 0.f) return;

    LaserScan& scan = ready_result_.scan;
    scan = LaserScan{};
    // The header's 16-bit "time" field is a device counter of unverified unit
    // (the vendor header calls it "not enabled" and its ROS driver never
    // stamps a scan with it), while this field is contracted to be a device
    // clock in ms — leave it at 0 and report timing from the rotation below.
    scan.timestamp_ms = 0;
    scan.ranges      = std::move(pending_ranges_);
    scan.intensities = std::move(pending_intensities_);
    scan.angle_min       = (static_cast<float>(kSweepStartDeg) + cfg_.angle_offset_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.range_min = cfg_.range_min_m;
    scan.range_max = cfg_.range_max_m;

    // Timing: revolutions complete one rotation period apart, but the points
    // in one only span the 320° the head measures — the remaining 40° is dead
    // time before the next sweep starts. scan_time is what a consumer
    // subtracts from the arrival time to date the FIRST point, so it must be
    // the sweep, not the period (the vendor ROS driver stamps with the same
    // 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
    // 360/320 higher than the mechanical one. The first revolution has
    // nothing to measure against — leave the fields at 0 and let the consumer
    // fall back.
    const auto now = std::chrono::steady_clock::now();
    if (have_last_rev_) {
        const float period = std::chrono::duration<float>(now - last_rev_end_).count();
        if (period > 0.f && period < kMaxRevPeriodS) {
            scan.scan_time      = period * (kSweepSpanDeg / kFullTurnDeg);
            scan.time_increment = scan.scan_time / static_cast<float>(scan.ranges.size() - 1);
        }
    }
    last_rev_end_  = now;
    have_last_rev_ = true;

    finalize_scan(scan, cfg_, inverted_);

    ExtraInfo& info = ready_result_.info;
    info = ExtraInfo{};
    info.detected_model    = cfg_.name;
    info.espe_error_status = espe_error_status_;

    latest_diag_ = decode_diagnostics(info);
    mark_scan_decoded();

    begin_revolution();  // the vectors above were moved out — restore them
    scan_ready_ = true;
}

bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
    for (;;) {
        if (parse_buffer()) {
            scan_ready_ = false;
            out = std::move(ready_result_);
            set_error(ErrorCode::Ok);
            return true;
        }
        if (!fill_buffer(timeout_ms)) return false;
    }
}

bool EspeDriver::spin_once() {
    if (!parse_buffer()) {
        if (!fill_buffer(0)) return false;
        parse_buffer();
    }
    if (scan_ready_) {
        scan_ready_ = false;
        if (cb_) cb_(ready_result_);
    }
    return true;
}

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

namespace {

const DriverInfo kDriverInfo = [] {
    DriverInfo info;
    info.vendor      = "ESPE";
    info.model       = "LGA60";
    info.driver_id   = "espe_lga60_driver";
    info.description = "ESPE LGA60 320° laser scanner — TCP by default, UDP via "
                       "DeviceConfig::transport; open() sends the RAuto start "
                       "command; device parameters come from the vendor Windows "
                       "tool. Default port 8080 (vendor default IP 192.168.1.88). "
                       "Ported from the vendor ROS driver; not verified on real "
                       "hardware.";
    info.transport   = Transport::Tcp;
    info.transport_selectable = true;   // transport = udp switches to UDP
    info.supported_models = {"ESPE-LGA60"};
    return info;
}();

} // namespace

DriverInfo EspeDriver::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 : 8080;
    const bool     use_udp = cfg->transport == Transport::Udp;
    return new EspeDriver(apply_device_config(MODEL_ESPE_LGA60, *cfg),
                          cfg->ip, port, use_udp, cfg->inverted);
}