#pragma once
#include "lidarlib/lidar.hpp"
#include <cstdint>
#include <string>
#include <vector>

namespace lidarlib {

// SICK TiM presets. FOV/range from datasheets; scan_angle_* are informational
// only and do NOT filter points. angle_offset_deg = -90 because the TiM wire
// frame puts 90° at the device front.
inline constexpr ModelConfig MODEL_SICK_TIM5XX { "SICK-TIM5xx", -135.f, 135.f, 0.05f, 10.f, -90.f }; // TiM551/561, 270°, 10m
inline constexpr ModelConfig MODEL_SICK_TIM571 { "SICK-TIM571", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM571,      270°, 25m
inline constexpr ModelConfig MODEL_SICK_TIM7XX { "SICK-TIM7xx", -135.f, 135.f, 0.05f, 25.f, -90.f }; // TiM781,      270°, 25m

// SICK TiM5xx/7xx over SOPAS/CoLa-A (TCP, default port 2111).
// Verified against a real TiM781S (FW V5.11). NOT verified: NumEncoders > 0,
// the 8-bit channel branch, and the TIM5xx/TIM571 FOV/range numbers.
class SickDriver : public Lidar {
public:
    using ScanCallback = lidarlib::ScanCallback;

    explicit SickDriver(const ModelConfig& cfg,
                         const std::string& ip,
                         uint16_t            port = 2111);
    ~SickDriver();

    SickDriver(const SickDriver&)            = delete;
    SickDriver& operator=(const SickDriver&) = delete;

    // Connect + send "sEN LMDscandata 1" to start continuous scan output.
    bool open() override;
    void close() override;
    bool recv_scan(ScanResult& out, int timeout_ms = 2000) override;
    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
    bool spin_once() override;

    // No model string on the wire — returns the configured name.
    const char* detected_model() const override { return detected_model_name_.c_str(); }

private:
    bool send_telegram(const std::string& body);
    bool read_telegram(std::string& out, int timeout_ms);
    bool parse_lmdscandata(const std::string& telegram, ScanResult& out);

    ModelConfig  cfg_;
    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
    std::string  ip_;
    uint16_t     port_;
    int          sock_fd_ = -1;
    ScanCallback cb_;

    // Leftover TCP bytes carried across telegram boundaries; per-instance.
    std::string recv_buf_;
};

inline constexpr ModelConfig MODEL_SICK_NANOSCAN3 { "SICK-nanoScan3", -137.5f, 137.5f, 0.05f, 40.f };

// SICK nanoScan3 / microScan3 safety-scanner binary UDP output. Layout ported
// from SICK's open-source sick_safetyscanners; NOT verified on real hardware.
// Passive UDP receiver: the sensor's UDP target must be configured up front in
// SICK Safety Designer — this class does no CoLa2/TCP handshake.
class NanoScanDriver : public Lidar {
public:
    using ScanCallback = lidarlib::ScanCallback;

    // ip: local bind address; port: local UDP port the sensor sends to.
    explicit NanoScanDriver(const ModelConfig& cfg,
                            const std::string& ip   = "0.0.0.0",
                            uint16_t            port = 6060);
    ~NanoScanDriver();

    NanoScanDriver(const NanoScanDriver&)            = delete;
    NanoScanDriver& operator=(const NanoScanDriver&) = delete;

    bool open() override;
    void close() override;
    bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
    void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
    bool spin_once() override;

    // No model string on the wire — returns the configured name.
    const char* detected_model() const override { return detected_model_name_.c_str(); }

private:
    int  recv_datagram(int timeout_ms);
    bool parse_packet(const uint8_t* buf, int len, ScanResult& out);

    ModelConfig  cfg_;
    std::string  detected_model_name_;  // owned copy of cfg_.name (stable lifetime)
    std::string  ip_;
    uint16_t     port_;
    int          sock_fd_ = -1;
    ScanCallback cb_;

    // Per-instance; sized for a full safety-data packet (max ~2751 beams).
    std::vector<uint8_t> recv_buf_;
};

} // namespace lidarlib