from __future__ import annotations

from pathlib import Path
import struct

import numpy as np


class PCDFormatError(ValueError):
    pass


def _lzf_decompress(data: bytes, expected: int) -> bytes:
    out = bytearray()
    i = 0
    while i < len(data):
        ctrl = data[i]
        i += 1
        if ctrl < 32:
            length = ctrl + 1
            if i + length > len(data):
                raise PCDFormatError("truncated LZF literal")
            out.extend(data[i:i + length])
            i += length
        else:
            length = ctrl >> 5
            ref = len(out) - ((ctrl & 0x1f) << 8) - 1
            if length == 7:
                if i >= len(data):
                    raise PCDFormatError("truncated LZF length")
                length += data[i]
                i += 1
            if i >= len(data):
                raise PCDFormatError("truncated LZF offset")
            ref -= data[i]
            i += 1
            length += 2
            if ref < 0:
                raise PCDFormatError("invalid LZF back reference")
            for _ in range(length):
                out.append(out[ref])
                ref += 1
    if len(out) != expected:
        raise PCDFormatError(f"LZF decoded {len(out)} bytes, expected {expected}")
    return bytes(out)


def read_pcd(path: str | Path) -> tuple[dict[str, list[str]], np.ndarray]:
    """Read PCD ASCII, binary, or binary_compressed into an Nx4 float32 XYZI.

    XYZ and intensity fields are selected by name. Missing intensity is NaN.
    Organized dimensions are exposed in the returned header (`WIDTH`, `HEIGHT`).
    """
    path = Path(path)
    with path.open("rb") as f:
        header: dict[str, list[str]] = {}
        while True:
            line = f.readline()
            if not line:
                raise PCDFormatError("missing DATA line")
            try:
                text = line.decode("ascii").strip()
            except UnicodeDecodeError as exc:
                raise PCDFormatError("non-ASCII PCD header") from exc
            if not text or text.startswith("#"):
                continue
            parts = text.split()
            header[parts[0].upper()] = parts[1:]
            if parts[0].upper() == "DATA":
                break
        fields = header.get("FIELDS", header.get("FIELD", []))
        sizes = list(map(int, header.get("SIZE", ["4"] * len(fields))))
        types = header.get("TYPE", ["F"] * len(fields))
        counts = list(map(int, header.get("COUNT", ["1"] * len(fields))))
        width = int(header.get("WIDTH", ["0"])[0])
        height = int(header.get("HEIGHT", ["1"])[0])
        points = int(header.get("POINTS", [str(width * height)])[0])
        if points <= 0 or not fields or not (len(fields) == len(sizes) == len(types) == len(counts)):
            raise PCDFormatError("invalid PCD dimensions or field metadata")
        offsets = {}
        stride = 0
        for name, size, typ, count in zip(fields, sizes, types, counts):
            offsets[name.lower()] = (stride, size, typ.upper(), count)
            stride += size * count
        payload = f.read()
    mode = header["DATA"][0].lower()
    if mode == "ascii":
        table = np.loadtxt(path, comments="#", skiprows=0) if False else None
        # Decode the payload directly; header text has already been consumed.
        try:
            raw = np.fromstring(payload.decode("ascii"), sep=" ", dtype=np.float64).reshape(points, sum(counts))
        except (ValueError, UnicodeDecodeError) as exc:
            raise PCDFormatError("invalid ASCII PCD payload") from exc
        col = 0
        columns = {}
        for name, count in zip(fields, counts):
            columns[name.lower()] = raw[:, col]
            col += count
    elif mode in ("binary", "binary_compressed"):
        if mode == "binary_compressed":
            if len(payload) < 8:
                raise PCDFormatError("truncated compressed PCD")
            compressed_size, uncompressed_size = struct.unpack("<II", payload[:8])
            decoded = _lzf_decompress(payload[8:8 + compressed_size], uncompressed_size)
            # PCL compressed layout stores each field contiguously (structure of arrays).
            columns = {}
            off = 0
            for name, size, typ, count in zip(fields, sizes, types, counts):
                dtype = _dtype(typ, size)
                values = np.frombuffer(decoded, dtype=dtype, count=points * count, offset=off)
                columns[name.lower()] = values.reshape(points, count)[:, 0]
                off += points * count * size
        else:
            if len(payload) < points * stride:
                raise PCDFormatError("truncated binary PCD payload")
            columns = {}
            off = 0
            for name, size, typ, count in zip(fields, sizes, types, counts):
                dtype = _dtype(typ, size)
                values = np.ndarray((points, count), dtype=dtype, buffer=payload, offset=off, strides=(stride, size))
                columns[name.lower()] = values[:, 0]
                off += size * count
    else:
        raise PCDFormatError(f"unsupported PCD DATA mode: {mode}")
    if not all(k in columns for k in ("x", "y", "z")):
        raise PCDFormatError("PCD requires x, y, and z fields")
    intensity = columns.get("intensity", np.full(points, np.nan))
    return header, np.column_stack([columns["x"], columns["y"], columns["z"], intensity]).astype(np.float32)


def _dtype(typ: str, size: int) -> np.dtype:
    if typ == "F" and size in (4, 8):
        code = "f" if size == 4 else "d"
    elif typ in ("I", "U") and size in (1, 2, 4, 8):
        code = ("i" if typ == "I" else "u") + str(size)
        return np.dtype("<" + code)
    else:
        raise PCDFormatError(f"unsupported PCD field type {typ}{size}")
    return np.dtype("<" + code)
