cloudini 0.3.1

//! ROS integration — compress and decompress [`PointCloud2Msg`] using the Cloudini format.
//!
//! Enable the `ros` feature in your `Cargo.toml`:
//!
//! ```toml
//! [dependencies]
//! cloudini = { version = "*", features = ["ros"] }
//! ```
//!
//! ## Compressing a cloud
//!
//! ```rust
//! use cloudini::ros::{CompressExt, CompressionConfig, PointCloud2Msg};
//! use ros_pointcloud2::points::PointXYZ;
//!
//! # let cloud_msg = PointCloud2Msg::try_from_iter(&[PointXYZ::new(1.0, 2.0, 3.0)]).unwrap();
//! // one line — 1 mm resolution, ZSTD compression
//! let compressed = cloud_msg.compress(CompressionConfig::lossy_zstd(0.001)).unwrap();
//!
//! // ship `compressed` over the network or store it
//! ```
//!
//! ## Decompressing
//!
//! ```rust
//! use cloudini::ros::{CompressExt, CompressionConfig, PointCloud2Msg};
//! use ros_pointcloud2::points::PointXYZ;
//!
//! # let cloud_msg = PointCloud2Msg::try_from_iter(&[PointXYZ::new(1.0, 2.0, 3.0)]).unwrap();
//! # let compressed = cloud_msg.compress(CompressionConfig::default()).unwrap();
//! let cloud_msg = compressed.decompress().unwrap();
//! // cloud_msg is a PointCloud2Msg ready for downstream processing
//! ```
//!
//! ## Re-exported types
//!
//! Common `ros_pointcloud2` types are re-exported here so you only need one import:
//! [`PointCloud2Msg`], [`HeaderMsg`], [`PointFieldMsg`], [`TimeMsg`],
//! [`FieldDatatype`], [`CloudDimensions`], [`Endian`], [`Denseness`].

// Re-export core ros_pointcloud2 types so callers need only `cloudini::ros::*`.
// These `pub use` also bring the names into scope for use within this module.
pub use ros_pointcloud2::ros::{HeaderMsg, PointFieldMsg, TimeMsg};
pub use ros_pointcloud2::{CloudDimensions, Denseness, Endian, FieldDatatype, PointCloud2Msg};

use crate::{
    CompressionOption, EncodingInfo, EncodingOptions, FieldType, PointField, PointcloudDecoder,
    PointcloudEncoder,
};

/// Controls how a [`PointCloud2Msg`] is encoded and compressed.
///
/// Use the smart constructors for common cases, or build one manually for
/// custom per-field resolution control via [`CompressionConfig::with_resolution`].
///
/// ## Examples
///
/// ```rust
/// use cloudini::ros::CompressionConfig;
///
/// // 1 mm lossy + ZSTD (typical Lidar geometry)
/// let cfg = CompressionConfig::lossy_zstd(0.001);
///
/// // 1 cm lossy + LZ4 (real-time, slightly larger files)
/// let cfg = CompressionConfig::lossy_lz4(0.01);
///
/// // lossless XOR-encoding + ZSTD (preserves every float bit)
/// let cfg = CompressionConfig::lossless_zstd();
/// ```
#[derive(Clone, Debug)]
pub struct CompressionConfig {
    /// Field-level encoding algorithm.
    pub encoding: EncodingOptions,
    /// Block compression applied after field encoding.
    pub compression: CompressionOption,
    /// Quantisation step for all float fields (`None` → copy verbatim / XOR lossless).
    pub resolution: Option<f32>,
}

impl CompressionConfig {
    /// Lossy float quantisation + ZSTD block compression.
    ///
    /// `resolution` is the quantisation step (e.g. `0.001` for 1 mm precision).
    /// Maximum per-component error is `resolution / 2`.
    pub fn lossy_zstd(resolution: f32) -> Self {
        Self {
            encoding: EncodingOptions::Lossy,
            compression: CompressionOption::Zstd,
            resolution: Some(resolution),
        }
    }

    /// Lossy float quantisation + LZ4 block compression.
    ///
    /// Faster than [`Self::lossy_zstd`] at the cost of a slightly larger output.
    pub fn lossy_lz4(resolution: f32) -> Self {
        Self {
            encoding: EncodingOptions::Lossy,
            compression: CompressionOption::Lz4,
            resolution: Some(resolution),
        }
    }

    /// Lossless XOR encoding (Float64) + ZSTD block compression.
    ///
    /// Every float bit is preserved exactly. Use when downstream algorithms
    /// are sensitive to floating-point rounding (e.g. ICP, odometry).
    pub fn lossless_zstd() -> Self {
        Self {
            encoding: EncodingOptions::Lossless,
            compression: CompressionOption::Zstd,
            resolution: None,
        }
    }

    /// Lossless XOR encoding (Float64) + LZ4 block compression.
    pub fn lossless_lz4() -> Self {
        Self {
            encoding: EncodingOptions::Lossless,
            compression: CompressionOption::Lz4,
            resolution: None,
        }
    }

    /// Override the quantisation resolution after construction.
    ///
    /// Useful to start from a preset and then fine-tune:
    ///
    /// ```rust
    /// use cloudini::ros::CompressionConfig;
    /// let cfg = CompressionConfig::lossy_zstd(0.001).with_resolution(0.005);
    /// ```
    pub fn with_resolution(mut self, resolution: f32) -> Self {
        self.resolution = Some(resolution);
        self
    }

    /// Returns the format string stored in [`CompressedPointCloud2::format`].
    fn format_str(&self) -> String {
        let enc = match self.encoding {
            EncodingOptions::Lossy => "lossy",
            EncodingOptions::Lossless => "lossless",
            EncodingOptions::None => "none",
        };
        let comp = match self.compression {
            CompressionOption::Zstd => "zstd",
            CompressionOption::Lz4 => "lz4",
            CompressionOption::None => "none",
        };
        format!("cloudini/{enc}/{comp}")
    }
}

impl Default for CompressionConfig {
    /// Default: 1 mm lossy + ZSTD — good for most Lidar workloads.
    fn default() -> Self {
        Self::lossy_zstd(0.001)
    }
}

/// The `point_cloud_interfaces/msg/CompressedPointCloud2` ROS message type.
///
/// Mirrors the ROS `.msg` definition:
///
/// ```text
/// std_msgs/Header header
/// uint32 height
/// uint32 width
/// sensor_msgs/PointField[] fields
/// bool is_bigendian
/// uint32 point_step
/// uint32 row_step
/// uint8[] compressed_data
/// bool is_dense
/// string format
/// ```
///
/// `compressed_data` contains a self-contained Cloudini buffer (YAML header +
/// compressed chunks), so no out-of-band metadata is needed to decompress.
///
/// ## Example
///
/// ```rust
/// use cloudini::ros::{CompressedPointCloud2, CompressionConfig, PointCloud2Msg};
/// use ros_pointcloud2::points::PointXYZ;
///
/// # let cloud_msg = PointCloud2Msg::try_from_iter(&[PointXYZ::new(1.0, 2.0, 3.0)]).unwrap();
/// // Compress
/// let compressed = CompressedPointCloud2::compress(cloud_msg, CompressionConfig::default()).unwrap();
///
/// // Send compressed over DDS, store to disk, etc.
///
/// // Decompress
/// let restored: PointCloud2Msg = compressed.decompress().unwrap();
/// ```
#[derive(Clone, Debug)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(
    feature = "rkyv",
    derive(rkyv::Archive, rkyv::Serialize, rkyv::Deserialize)
)]
pub struct CompressedPointCloud2 {
    pub header: HeaderMsg,
    pub height: u32,
    pub width: u32,
    pub fields: Vec<PointFieldMsg>,
    pub is_bigendian: bool,
    pub point_step: u32,
    pub row_step: u32,
    /// Cloudini-compressed payload (header + chunk data).
    pub compressed_data: Vec<u8>,
    pub is_dense: bool,
    /// Human-readable compression descriptor, e.g. `"cloudini/lossy/zstd"`.
    pub format: String,
}

impl CompressedPointCloud2 {
    /// Compress a [`PointCloud2Msg`] into a [`CompressedPointCloud2`].
    ///
    /// The `config` controls quantisation and block compression. Use
    /// [`CompressionConfig::default()`] for 1 mm lossy + ZSTD.
    ///
    /// # Errors
    ///
    /// Returns an error if `point_step` is 0, the data length is inconsistent,
    /// or the compression library reports a failure.
    pub fn compress(msg: PointCloud2Msg, config: CompressionConfig) -> crate::Result<Self> {
        let format = config.format_str();
        let info = encoding_info_from_msg(&msg, &config);
        let encoder = PointcloudEncoder::new(info);
        let compressed_data = encoder.encode(&msg.data)?;

        let is_bigendian = matches!(msg.endian, Endian::Big);
        let is_dense = matches!(msg.dense, Denseness::Dense);

        Ok(Self {
            header: msg.header,
            height: msg.dimensions.height,
            width: msg.dimensions.width,
            fields: msg.fields,
            is_bigendian,
            point_step: msg.point_step,
            row_step: msg.row_step,
            compressed_data,
            is_dense,
            format,
        })
    }

    /// Decompress back to a [`PointCloud2Msg`].
    ///
    /// All metadata (width, height, fields, point_step) is recovered from the
    /// embedded Cloudini header — no config needed on the decode side.
    ///
    /// # Errors
    ///
    /// Returns an error if the `compressed_data` is malformed or truncated.
    pub fn decompress(&self) -> crate::Result<PointCloud2Msg> {
        let decoder = PointcloudDecoder::new();
        let (info, raw_data) = decoder.decode(&self.compressed_data)?;

        let fields = info
            .fields
            .iter()
            .map(|f| PointFieldMsg {
                name: f.name.clone().into(),
                offset: f.offset,
                datatype: field_type_to_datatype(f.field_type),
                count: 1,
            })
            .collect();

        Ok(PointCloud2Msg {
            header: self.header.clone(),
            dimensions: CloudDimensions {
                width: info.width,
                height: info.height,
            },
            fields,
            endian: if self.is_bigendian {
                Endian::Big
            } else {
                Endian::Little
            },
            point_step: info.point_step,
            row_step: info.point_step * info.width,
            data: raw_data,
            dense: if self.is_dense {
                Denseness::Dense
            } else {
                Denseness::Sparse
            },
        })
    }
}

/// Extension trait that adds `.compress()` directly to [`PointCloud2Msg`].
///
/// ```rust
/// use cloudini::ros::{CompressExt, CompressionConfig, PointCloud2Msg};
/// use ros_pointcloud2::points::PointXYZ;
///
/// # let cloud_msg = PointCloud2Msg::try_from_iter(&[PointXYZ::new(1.0, 2.0, 3.0)]).unwrap();
/// let compressed = cloud_msg.compress(CompressionConfig::lossy_zstd(0.001)).unwrap();
/// ```
pub trait CompressExt {
    /// Compress this cloud. See [`CompressionConfig`] for options.
    fn compress(self, config: CompressionConfig) -> crate::Result<CompressedPointCloud2>;
}

impl CompressExt for PointCloud2Msg {
    fn compress(self, config: CompressionConfig) -> crate::Result<CompressedPointCloud2> {
        CompressedPointCloud2::compress(self, config)
    }
}

/// Build an [`EncodingInfo`] from the message layout + compression config.
fn encoding_info_from_msg(msg: &PointCloud2Msg, config: &CompressionConfig) -> EncodingInfo {
    let fields = msg
        .fields
        .iter()
        .map(|f| {
            let field_type = datatype_to_field_type(f.datatype);
            // Apply resolution only to float fields; integers are always lossless delta-varint.
            let resolution = match field_type {
                FieldType::Float32 | FieldType::Float64 => config.resolution,
                _ => None,
            };
            PointField {
                name: f.name.as_str().to_owned(),
                offset: f.offset,
                field_type,
                resolution,
            }
        })
        .collect();

    EncodingInfo {
        fields,
        width: msg.dimensions.width,
        height: msg.dimensions.height,
        point_step: msg.point_step,
        encoding_opt: config.encoding,
        compression_opt: config.compression,
        ..EncodingInfo::default()
    }
}

fn datatype_to_field_type(datatype: u8) -> FieldType {
    match datatype {
        1 => FieldType::Int8,
        2 => FieldType::Uint8,
        3 => FieldType::Int16,
        4 => FieldType::Uint16,
        5 => FieldType::Int32,
        6 => FieldType::Uint32,
        7 => FieldType::Float32, // also covers packed RGB
        8 => FieldType::Float64,
        _ => FieldType::Unknown,
    }
}

fn field_type_to_datatype(ft: FieldType) -> u8 {
    match ft {
        FieldType::Int8 => 1,
        FieldType::Uint8 => 2,
        FieldType::Int16 => 3,
        FieldType::Uint16 => 4,
        FieldType::Int32 => 5,
        FieldType::Uint32 => 6,
        FieldType::Float32 => 7,
        FieldType::Float64 => 8,
        // Int64/Uint64 have no standard sensor_msgs/PointField datatype code
        FieldType::Int64 | FieldType::Uint64 | FieldType::Unknown => 0,
    }
}