#include "lidarlib/lidar.hpp"

#include <cstring>
#include <cmath>
#include <stdexcept>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/select.h>

namespace lidarlib {

namespace {
constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
}

// ── Little-endian helpers ────────────────────────────────────────────────────
static inline uint16_t le16(const uint8_t* p) {
    return static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8);
}
static 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);
}

// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead,
// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly
// filter lidars that report angles in 0–360 too.
static inline float to_signed_deg(float deg) {
    deg = std::fmod(deg, 360.f);
    if (deg < 0.f)    deg += 360.f;   // → [0,360)
    if (deg > 180.f)  deg -= 360.f;   // → (-180,180]
    return deg;
}

// Mirror the angle when the unit is mounted upside-down (flipped 180° about
// its forward axis), so output angle stays correct relative to the vehicle
// frame regardless of physical mounting. Must run AFTER to_signed_deg() and
// BEFORE the FOV filter, since the FOV window is defined in vehicle frame.
static inline float maybe_invert(float signed_deg, bool inverted) {
    return inverted ? to_signed_deg(-signed_deg) : signed_deg;
}

// ── CRC32 (poly 0x04C11DB7, MSB-first) ──────────────────────────────────────
static uint32_t crc32_olei(const uint8_t* data, size_t len) {
    uint32_t crc = 0xFFFFFFFF;
    for (size_t i = 0; i < len; ++i) {
        crc ^= static_cast<uint32_t>(data[i]) << 24;
        for (int b = 0; b < 8; ++b)
            crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1);
    }
    return crc;
}

// ── Frame IDs ────────────────────────────────────────────────────────────────
static constexpr uint16_t FRAME_ID_A = 0xFAF0;  // 2D Ethernet (VB, VF, LR-1F)
static constexpr uint16_t FRAME_ID_B = 0xFEF0;  // LR-1BS5 / LR-1BS2 Ethernet variant
static constexpr uint16_t FRAME_ID_C = 0xFEAC;  // Protocol V3 (GS1-5)

// ─── Constructor / Destructor ────────────────────────────────────────────────
Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
    : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
{
    auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
}

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

// ─── open() ─────────────────────────────────────────────────────────────────
bool Driver::open() {
    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
    if (sock_fd_ < 0) return false;

    // Allow multiple sockets to bind the same port (run alongside another
    // app / debugging). SO_REUSEPORT lets several listeners receive the same
    // UDP stream — only works if EVERY socket on that port sets this flag.
    int reuse = 1;
    ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
#ifdef SO_REUSEPORT
    ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
#endif

    sockaddr_in addr{};
    addr.sin_family      = AF_INET;
    addr.sin_port        = htons(port_);
    addr.sin_addr.s_addr = inet_addr(ip_.c_str());

    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
        ::close(sock_fd_);
        sock_fd_ = -1;
        return false;
    }
    pending_angle_deg_.reserve(2048);
    pending_dist_m_.reserve(2048);
    pending_intensity_.reserve(2048);
    return true;
}

// ─── close() ────────────────────────────────────────────────────────────────
void Driver::close() {
    if (sock_fd_ >= 0) {
        ::close(sock_fd_);
        sock_fd_ = -1;
    }
}

// ─── recv_scan() — blocks until one full revolution is available ──────────
bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
    scan_ready_ = false;

    while (!scan_ready_) {
        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) return false;  // timeout or error
        }
        if (!spin_once()) return false;
    }
    out = std::move(ready_result_);
    return true;
}

// ─── spin_once() ────────────────────────────────────────────────────────────
bool Driver::spin_once() {
    // buf is the recv_buf_ member, NOT static → each Driver has its own
    // memory, safe when 2 lidars receive concurrently on 2 threads.
    uint8_t* buf = recv_buf_;
    sockaddr_in from{};
    socklen_t   fromlen = sizeof(from);

    ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
                            reinterpret_cast<sockaddr*>(&from), &fromlen);
    if (n < 0) return false;

    // Distinguish protocol family by Frame ID (little-endian)
    // Family A / C: Frame ID / magic sits right at bytes [0-1]
    // Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3]
    if (n < 4) return true;  // too short, skip
    uint16_t id_at_0 = le16(buf);        // Family A (0xFAF0) or Family C (0xFEAC)
    uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3]

    if      (id_at_0 == FRAME_ID_A)    parse_family_a(buf, static_cast<int>(n));
    else if (id_at_0 == FRAME_ID_C)    parse_family_c(buf, static_cast<int>(n));
    else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n));
    // else: unknown family (3D LR-16F uses a different format, extend later)

    return true;
}

// ─── push_point() — append with angle-unwrapping ───────────────────────────
// `signed_angle_deg` is already signed+inverted+FOV-filtered by the caller.
// Unwrapping against the previous point (rather than re-deriving from device
// raw angle) keeps this identical for all 3 families and survives the ±180°
// seam: a 360° device's points cross from +179.x to -179.x mid-revolution in
// the signed system, which push_point() turns back into a continuous ramp so
// LaserScan::angle_min/angle_max/ranges stay meaningful (monotonic, ROS-style).
void Driver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
    float angle = signed_angle_deg;
    if (!pending_angle_deg_.empty()) {
        float prev = pending_angle_deg_.back();
        while (angle - prev >  180.f) angle -= 360.f;
        while (angle - prev < -180.f) angle += 360.f;
    }
    pending_angle_deg_.push_back(angle);
    pending_dist_m_.push_back(dist_m);
    pending_intensity_.push_back(intensity);
}

// ─── flush_scan() — a revolution is complete ───────────────────────────────
void Driver::flush_scan() {
    if (pending_angle_deg_.empty()) return;

    const size_t n = pending_angle_deg_.size();

    LaserScan& scan      = ready_result_.scan;
    scan.timestamp_ms     = pending_ts_;
    scan.angle_min        = pending_angle_deg_.front() * kDeg2Rad;
    scan.angle_max        = pending_angle_deg_.back()  * kDeg2Rad;
    scan.angle_increment  = (n > 1)
        ? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
    scan.time_increment   = 0.f;  // device doesn't expose per-point timing
    scan.scan_time        = 0.f;  // device doesn't expose per-scan timing
    scan.range_min        = cfg_.range_min_m;
    scan.range_max        = cfg_.range_max_m;
    scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
    scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());

    ExtraInfo& info  = ready_result_.info;
    info             = pending_info_;
    info.detected_model = detected_model_name_;
    info.error_status    = pending_err_;

    pending_angle_deg_.clear();
    pending_dist_m_.clear();
    pending_intensity_.clear();
    pending_info_ = ExtraInfo{};  // reset per-revolution optional fields
    scan_ready_   = true;

    if (cb_) cb_(ready_result_);
}

// ─── parse_family_a() ───────────────────────────────────────────────────────
// 20-byte header:
//   [0-1]  Frame ID   = 0xFAF0
//   [2-3]  Protocol   = 0x0200
//   [4]    Distance scale (mm/count)
//   [5]    Error status
//   [6]    Start angle (deg, uint8)
//   [7]    End angle   (deg, uint8, exclusive)
//   [8-9]  Num points  (uint16 LE)
//   [10-11] Rotation info — raw, undecoded (exposed as ExtraInfo::rotation_raw)
//   [12-15] Timestamp  (uint32 LE, ms)
//   [16-19] CRC32 of the block data
// 3-byte block × N:
//   [0-1]  Distance readout (uint16 LE)
//   [2]    Intensity (uint8)
bool Driver::parse_family_a(const uint8_t* buf, int len) {
    static constexpr int HEADER_LEN = 20;
    static constexpr int BLOCK_LEN  =  3;

    if (len < HEADER_LEN) return false;

    // ── read header ──
    // uint16_t protocol  = le16(buf + 2);   // 0x0200
    uint8_t  dist_scale = buf[4];           // mm per count
    uint8_t  err_status = buf[5];
    float    ang_start  = static_cast<float>(buf[6]);
    // float ang_end    = static_cast<float>(buf[7]);  // exclusive
    uint16_t num_pts    = le16(buf + 8);
    uint16_t rotation_raw = le16(buf + 10);
    uint32_t timestamp  = le32(buf + 12);
    uint32_t crc_packet = le32(buf + 16);

    // ── verify CRC (optional but recommended) ──
    int block_bytes = len - HEADER_LEN;
    if (block_bytes < num_pts * BLOCK_LEN) return false;  // truncated packet

    uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(num_pts * BLOCK_LEN));
    if (crc_calc != crc_packet) return false;  // CRC mismatch

    // ── detect wrap-around → flush the previous revolution ──
    if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
        flush_scan();
    }

    // ── decode points ──
    pending_ts_  = timestamp;
    pending_err_ = err_status;
    pending_info_.distance_scale_mm = dist_scale;
    pending_info_.rotation_raw      = rotation_raw;

    // scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0.
    const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
    const float ang_end  = static_cast<float>(buf[7]);

    const uint8_t* blk = buf + HEADER_LEN;
    for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
        uint16_t dist_raw = le16(blk);
        uint8_t  intensity = blk[2];

        // Compute angle: linear interpolation within the packet's range (device-space)
        float frac  = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
        float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start));
        angle       = maybe_invert(angle, inverted_);

        // Filter out anything outside the model's FOV (already in the signed -180…180 system)
        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;

        push_point(angle, dist_raw * scale_mm * 0.001f /* mm → m */, intensity);
    }

    last_angle_ = ang_start;
    return true;
}

// ─── parse_family_b() ───────────────────────────────────────────────────────
// 40-byte header:
//   [0-1]  0x010F
//   [2-3]  0xFEF0  (Frame ID)
//   [4-5]  0x0200  (Protocol)
//   [6]    Distance scale
//   [7-16] Model identifier string (e.g. "OLELR-1BS5")
//   [17-39] Reserved
// 8-byte block × N:
//   [0-1]  AngleRaw (uint16 LE, × 0.01° → deg, 0–359.99); >= 0xFF00 = invalid point
//   [2-3]  Distance readout (uint16 LE); meters = value × DistanceScale / 1000
//   [4-5]  Signal strength (uint16 LE)
//   [6-7]  Reserved
// NOTE: this header carries no timestamp/error field, so ScanResult::scan's
// timestamp_ms and info.error_status stay at their defaults (0) for Family B.
bool Driver::parse_family_b(const uint8_t* buf, int len) {
    static constexpr int HEADER_LEN = 40;
    static constexpr int BLOCK_LEN  =  8;

    if (len < HEADER_LEN) return false;

    uint8_t dist_scale = buf[6];
    // scale=0 → default to 1 mm/count so distances don't collapse to zero.
    const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
    pending_info_.distance_scale_mm = dist_scale;
    if (auto_detect_ && !model_locked_) {
        std::string raw(reinterpret_cast<const char*>(buf + 7), 10);
        size_t z = raw.find('\0');
        if (z != std::string::npos) raw.resize(z);

        if (!raw.empty()) {
            detected_model_name_ = raw;
            model_locked_        = true;

            static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = {
                { "1BS5", &MODEL_LR1BS5 },
                { "16F",  &MODEL_LR16F  },
                { "1FMI", &MODEL_LR1FMI },  // must precede "1F": "OLELR-1FMI" also contains "1F"
                { "1F",   &MODEL_LR1F   },
                { "VF",   &MODEL_VF     },
                { "VB",   &MODEL_VB     },
            };
            for (const auto& entry : kModelTable) {
                if (raw.find(entry.key) != std::string::npos) {
                    cfg_.scan_angle_min = entry.cfg->scan_angle_min;
                    cfg_.scan_angle_max = entry.cfg->scan_angle_max;
                    cfg_.range_min_m    = entry.cfg->range_min_m;
                    cfg_.range_max_m    = entry.cfg->range_max_m;
                    break;
                }
            }
        }
    }

    int num_pts = (len - HEADER_LEN) / BLOCK_LEN;
    if (num_pts <= 0) return false;

    const uint8_t* blk = buf + HEADER_LEN;
    // AngleRaw is 0.01°/LSB (0–359.99°), per the official Olei block spec —
    // verified against real OLELR-1FMI geometry (a 0.25° scale smears a room
    // into a circle). AngleRaw >= 0xFF00 marks an invalid point → skip it.
    // The counter resets to 0 each revolution, but one packet is only a ~22°
    // arc and the device can pack >1 revolution across packets, so the
    // revolution boundary is detected PER POINT: a >90° drop between
    // consecutive [0,360) angles ends the current revolution.
    static constexpr uint16_t INVALID_ANGLE = 0xFF00;
    for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
        uint16_t angle_raw = le16(blk);
        if (angle_raw >= INVALID_ANGLE) continue;                        // invalid point

        float    dev_deg   = std::fmod(angle_raw * 0.01f, 360.f);        // [0,360)
        if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
            flush_scan();                                                 // revolution complete
        }
        last_angle_ = dev_deg;

        float    angle     = maybe_invert(to_signed_deg(angle_raw * 0.01f), inverted_);  // -180…180
        float    dist_m    = le16(blk + 2) * scale_mm * 0.001f;          // readout × scale → m
        uint8_t  intensity = static_cast<uint8_t>(le16(blk + 4) >> 2);   // 10-bit → 8-bit

        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;

        push_point(angle, dist_m, intensity);
    }

    return true;
}

// ─── parse_family_c() ───────────────────────────────────────────────────────
// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C#
// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently
// sniffed/verified against real GS1-5 hardware (no device was available to
// test this while writing the code).
// 48-byte header:
//   [0-1]   Magic        = 0xFEAC
//   [2-3]   Version
//   [4-7]   PacketSize   (uint32 LE)
//   [8-9]   HeaderSize   (uint16 LE, usually = 48)
//   [10]    Distance ratio — read by the original C# driver but NOT applied
//           (distance is always raw mm / 1000); same behavior kept here.
//           Exposed raw as ExtraInfo::distance_ratio_raw.
//   [11]    Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity),
//           0x10=4B/point (first 2 bytes unused, range at [+2,+4))
//   [12-13] Scan number    [14-15] Packet number
//   [16-19] Timestamp decimal   [20-23] Timestamp integer
//   [24-25] Scan frequency raw  [26-27] NumPointsScan (total points per revolution)
//   [28-29] Input status        [30-31] Output status
//   [32-35] Field status
//   [36-37] StartIndex   [38-39] EndIndex
//   [40-41] FirstIndex — index of this packet's first point within the full revolution
//   [42-43] NumPointsPacket — number of points in this packet
//   [44-47] Status flags
// All of [10], [24-25], [28-35], [44-47] are read and passed through raw in
// ExtraInfo — none of these are cross-verified against real hardware, same
// caveat as the rest of this family.
// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in
// the signed system (-180..180); no fmod needed like Family B since the
// index always stays within [0, NumPointsScan).
bool Driver::parse_family_c(const uint8_t* buf, int len) {
    static constexpr int HEADER_LEN = 48;
    if (len < HEADER_LEN) return false;

    uint16_t header_size_field = le16(buf + 8);
    uint8_t  distance_ratio_raw = buf[10];
    uint8_t  types             = buf[11];
    uint16_t scan_frequency_raw = le16(buf + 24);
    uint16_t num_pts_scan      = le16(buf + 26);
    uint16_t input_status      = le16(buf + 28);
    uint16_t output_status     = le16(buf + 30);
    uint32_t field_status      = le32(buf + 32);
    uint16_t first_index       = le16(buf + 40);
    uint16_t num_pts_packet    = le16(buf + 42);
    uint32_t status_flags      = le32(buf + 44);

    if (num_pts_scan == 0) return false;  // avoid divide-by-zero

    int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
    if (header_size < HEADER_LEN || header_size > len) return false;

    int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
    if (bytes_per_point == 0) return false;  // unknown Types, layout unclear

    int payload_bytes = len - header_size;
    int num_pts        = num_pts_packet;
    if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) {
        num_pts = payload_bytes / bytes_per_point;
    }
    if (num_pts <= 0) return false;

    pending_info_.distance_ratio_raw  = distance_ratio_raw;
    pending_info_.scan_frequency_raw  = scan_frequency_raw;
    pending_info_.input_status        = input_status;
    pending_info_.output_status       = output_status;
    pending_info_.field_status        = field_status;
    pending_info_.status_flags        = status_flags;

    // Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name
    // string in the header like Family B, but recognizing this family is
    // already enough to know the model, so auto-detect resolves immediately
    // without reading any extra field.
    if (auto_detect_ && !model_locked_) {
        cfg_.scan_angle_min   = MODEL_GS15.scan_angle_min;
        cfg_.scan_angle_max   = MODEL_GS15.scan_angle_max;
        cfg_.range_min_m      = MODEL_GS15.range_min_m;
        cfg_.range_max_m      = MODEL_GS15.range_max_m;
        detected_model_name_  = MODEL_GS15.name;
        model_locked_         = true;
    }

    const float angle_inc = 360.f / static_cast<float>(num_pts_scan);
    // raw_angle is used for wrap-around detection: it does NOT have the -180
    // offset that the externally-exposed angle gets, and stays in [0,360),
    // monotonically increasing — matching the same convention used by
    // Family A/B (last_angle_ >= 0 means "we already have a previous value");
    // subtracting 180 here could go negative and break that sentinel check.
    float raw_first_angle = static_cast<float>(first_index) * angle_inc;

    if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) {
        flush_scan();
    }

    const uint8_t* blk = buf + header_size;
    for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) {
        uint16_t range_mm;
        uint16_t inten_raw = 0;
        bool     has_inten = false;

        if (types == 0x00) {
            range_mm = le16(blk);
        } else if (types == 0x01) {
            range_mm  = le16(blk);
            inten_raw = le16(blk + 2);
            has_inten = true;
        } else {  // 0x10
            range_mm = le16(blk + 2);
        }

        float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f);
        angle       = maybe_invert(angle, inverted_);
        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;

        push_point(angle, range_mm * 0.001f /* mm → m */,
                   has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
    }

    last_angle_ = raw_first_angle;
    return true;
}

} // namespace lidarlib