// Internal helpers shared by the driver TUs — not part of the public API.
#pragma once
#include "lidarlib/lidar.hpp"
#include <cstdint>
#include <cstring>

namespace lidarlib {

inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;

// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
    const float new_min  = min_deg * kDeg2Rad;
    const float new_max  = max_deg * kDeg2Rad;
    const float old_span = scan.angle_max - scan.angle_min;
    if (old_span > 0.f)
        scan.angle_increment *= (new_max - new_min) / old_span;
    scan.angle_min = new_min;
    scan.angle_max = new_max;
}

// Little-endian readers (bounds are the caller's responsibility).
inline uint8_t  le_u8 (const uint8_t* p) { return p[0]; }
inline uint16_t le16(const uint8_t* p) {
    return static_cast<uint16_t>(p[0] | (p[1] << 8));
}
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);
}
inline int32_t  le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }

inline float bits_to_float(uint32_t bits) {
    float f;
    std::memcpy(&f, &bits, sizeof(f));
    return f;
}

} // namespace lidarlib