# LICENSE HEADER MANAGED BY add-license-header
#
# Copyright 2018 Kornia Team
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#

"""Read and write point clouds as ASCII or binary PLY files of x, y, z vertices.

The writers store one ``vertex`` per row of a channels-last ``(*, 3)`` tensor, with only the ``double`` properties
``x``, ``y`` and ``z``. The readers parse the PLY header, read the ``x``, ``y`` and ``z`` of the ``vertex`` element
whatever their scalar type, skip other vertex properties such as normals and colours and other elements such as
faces (each reader says which ``list`` properties it rejects), and return a float32 ``(N, 3)`` tensor on the CPU.
Every function takes a ``str`` or ``os.PathLike[str]`` ending in ``.ply``.

Which function to use:

- A text file: :func:`save_pointcloud_ply` writes one, :func:`load_pointcloud_ply` reads one.
- A binary file: :func:`save_pointcloud_ply_binary` writes one in little-endian float64, 24 bytes per point, and
  :func:`load_pointcloud_ply_binary` reads one in either byte order and with any PLY scalar type.
- The points of a depth map, to write: :func:`~kornia.geometry.depth.depth_to_3d_v2` returns them channels-last.
"""

from __future__ import annotations

import array
import os
import struct
import sys
import warnings
from typing import BinaryIO, Dict, List, NamedTuple, Optional, Tuple

import torch

__all__ = [
    "load_pointcloud_ply",
    "load_pointcloud_ply_binary",
    "save_pointcloud_ply",
    "save_pointcloud_ply_binary",
]


def save_pointcloud_ply(filename: str | os.PathLike[str], pointcloud: torch.Tensor) -> None:
    r"""Write a point cloud to an ASCII PLY file as double-precision x, y, z vertices.

    ``save_pointcloud_ply(filename, pointcloud)`` writes the points of a channels-last tensor ``(*, 3)`` with at least
    two dimensions, on any device, to a text PLY file, replacing any existing file, and returns ``None``. The file
    holds the coordinates only: no normals, colours or faces. Read it back with :func:`load_pointcloud_ply`, which
    returns float32, so float64 coordinates come back rounded to float32; for a binary file, use
    :func:`save_pointcloud_ply_binary` instead.

    Convention:
        - The file holds one ``vertex`` per row of ``pointcloud.reshape(-1, 3)``, in order and with none dropped,
          and declares the properties ``x``, ``y`` and ``z`` in that order as ``double``. Each value is printed as
          the shortest text that parses back to the same float64, a non-finite one as ``nan`` / ``inf``.
        - ``pointcloud`` is channels-last. Apply ``permute(0, 2, 3, 1)`` to a
          :func:`~kornia.geometry.depth.depth_to_3d` result or use :func:`~kornia.geometry.depth.depth_to_3d_v2`:
          a channels-first tensor whose width is 3 passes the shape check and is written scrambled.
        - The PLY writers and loaders take ``filename`` as a ``str`` or ``os.PathLike[str]`` ending in ``.ply``
          (any case); another suffix raises ``TypeError``.

    Args:
        filename: path of the ``.ply`` file to write, a ``str`` or ``os.PathLike[str]``.
        pointcloud: tensor of shape ``(*, 3)`` with at least two dimensions, where the last dimension is the
          point :math:`(X, Y, Z)`.

    Raises:
        TypeError: if ``filename`` is not a ``str`` or ``os.PathLike[str]`` ending in ``.ply``, if ``pointcloud`` is
            not a tensor, or if it has fewer than two dimensions or a last dimension other than 3.

    See Also:
        - :func:`load_pointcloud_ply`: reads the file back, as a float32 ``(N, 3)`` tensor.
        - :func:`save_pointcloud_ply_binary`: writes the same vertices as little-endian float64 instead of text.
        - :func:`~kornia.geometry.depth.depth_to_3d_v2`: returns the channels-last ``(*, H, W, 3)`` points of a
          depth map, ready to write.
    """
    filename = os.fspath(filename)
    if not (isinstance(filename, str) and filename.lower().endswith(".ply")):
        raise TypeError(f"Input filename must be a str or os.PathLike[str] with the .ply extension. Got {filename!r}")

    if not torch.is_tensor(pointcloud):
        raise TypeError(f"Input pointcloud type is not a torch.Tensor. Got {type(pointcloud)}")

    if pointcloud.ndim < 2 or pointcloud.shape[-1] != 3:
        raise TypeError(f"Input pointcloud must have shape (..., 3). Got {tuple(pointcloud.shape)}")

    # Flatten points. Every row is written, non-finite ones included, so row ``i`` of the file is
    # row ``i`` of the flattened input.
    xyz = pointcloud.reshape(-1, 3)
    num_points = xyz.shape[0]

    with open(filename, "w", encoding="utf-8") as f:
        # Write PLY header
        f.writelines(
            [
                "ply\n",
                "format ascii 1.0\n",
                "comment arraiy generated\n",
                f"element vertex {num_points}\n",
                "property double x\n",
                "property double y\n",
                "property double z\n",
                "end_header\n",
            ]
        )

        if num_points > 0:
            # Move to CPU, convert to float64 for matching 'double' in header
            arr = xyz.detach().cpu().to(torch.float64)
            # Write each row as space-separated floats. repr of a Python float is the
            # shortest string that round-trips it exactly, which a 'double' property
            # needs; a fixed significant-digit count silently loses float64 precision.
            for x, y, z in arr.tolist():
                f.write(f"{x!r} {y!r} {z!r}\n")


def save_pointcloud_ply_binary(filename: str | os.PathLike[str], pointcloud: torch.Tensor) -> None:
    r"""Write a point cloud to a little-endian binary PLY file as float64 x, y, z vertices.

    ``save_pointcloud_ply_binary(filename, pointcloud)`` writes the points of a channels-last tensor ``(*, 3)`` with
    at least two dimensions, on any device, to a ``binary_little_endian`` PLY file, 24 bytes per point,
    replacing any existing file, and returns ``None``. Read it back with :func:`load_pointcloud_ply_binary`; for a
    text file, use :func:`save_pointcloud_ply` instead.

    See the Convention block on :func:`save_pointcloud_ply`; the payload is little-endian float64.

    Args:
        filename: path of the ``.ply`` file to write, a ``str`` or ``os.PathLike[str]``.
        pointcloud: tensor of shape ``(*, 3)`` with at least two dimensions, where the last dimension is the
          point :math:`(X, Y, Z)`.

    Raises:
        TypeError: if ``filename`` is not a ``str`` or ``os.PathLike[str]`` ending in ``.ply``, if ``pointcloud`` is
            not a tensor, or if it has fewer than two dimensions or a last dimension other than 3.

    See Also:
        - :func:`load_pointcloud_ply_binary`: reads the file back, as a float32 ``(N, 3)`` tensor.
        - :func:`save_pointcloud_ply`: writes the same vertices as text instead.
    """
    filename = os.fspath(filename)
    if not (isinstance(filename, str) and filename.lower().endswith(".ply")):
        raise TypeError(f"Input filename must be a str or os.PathLike[str] with the .ply extension. Got {filename!r}")

    if not torch.is_tensor(pointcloud):
        raise TypeError(f"Input pointcloud type is not a torch.Tensor. Got {type(pointcloud)}")

    if pointcloud.ndim < 2 or pointcloud.shape[-1] != 3:
        raise TypeError(f"Input pointcloud must have shape (..., 3). Got {tuple(pointcloud.shape)}")

    # Flatten points. Every row is written, non-finite ones included, so row ``i`` of the file is
    # row ``i`` of the flattened input.
    xyz = pointcloud.reshape(-1, 3)
    num_points = xyz.shape[0]

    with open(filename, "wb") as f:
        # Write PLY header : Binary version
        header = [
            "ply\n",
            "format binary_little_endian 1.0\n",
            "comment kornia generated\n",
            f"element vertex {num_points}\n",
            "property double x\n",
            "property double y\n",
            "property double z\n",
            "end_header\n",
        ]
        f.writelines(s.encode("utf-8") for s in header)

        if num_points > 0:
            # Move to CPU, convert to float64 for matching 'double' in header
            arr = xyz.detach().cpu().to(torch.float64).reshape(-1)

            # Convert to array.array for efficient byte-level handling
            data_array = array.array("d", arr.tolist())

            # Ensure little-endian
            if sys.byteorder == "big":
                data_array.byteswap()

            # Write binary data in a single operation for I/O efficiency
            f.write(data_array.tobytes())


class _PlyElement(NamedTuple):
    name: str
    count: int
    # (property name, scalar type); ``None`` as the type marks a ``property list``
    properties: List[Tuple[str, Optional[str]]]


class _PlyHeader(NamedTuple):
    format: str
    elements: List[_PlyElement]


# PLY scalar types -> (struct code, torch dtype). Both the classic and the sized spellings are legal.
_PLY_SCALAR_TYPES: Dict[str, Tuple[str, Optional[torch.dtype]]] = {
    "char": ("b", torch.int8),
    "int8": ("b", torch.int8),
    "uchar": ("B", torch.uint8),
    "uint8": ("B", torch.uint8),
    "short": ("h", torch.int16),
    "int16": ("h", torch.int16),
    "ushort": ("H", None),
    "uint16": ("H", None),
    "int": ("i", torch.int32),
    "int32": ("i", torch.int32),
    "uint": ("I", None),
    "uint32": ("I", None),
    "float": ("f", torch.float32),
    "float32": ("f", torch.float32),
    "double": ("d", torch.float64),
    "float64": ("d", torch.float64),
}
_PLY_FORMATS = ("ascii", "binary_little_endian", "binary_big_endian")


def _read_ply_header(f: BinaryIO, filename: str) -> _PlyHeader:
    """Parse a PLY header up to and including ``end_header``.

    Leaves the file positioned at the first byte of the element data. The whole header is ASCII by
    definition of the format, whatever the payload encoding.
    """
    if f.readline().strip() != b"ply":
        raise ValueError(f"{filename!r} is not a PLY file: the first line must be 'ply'.")

    fmt: Optional[str] = None
    elements: List[_PlyElement] = []
    while True:
        raw = f.readline()
        if not raw:
            raise ValueError(f"PLY header in {filename!r} has no 'end_header' line.")
        tokens = raw.decode("ascii", errors="replace").split()
        if not tokens:
            continue
        keyword = tokens[0]
        if keyword == "end_header":
            break
        if keyword in ("comment", "obj_info"):
            continue
        if keyword == "format":
            if len(tokens) < 2 or tokens[1] not in _PLY_FORMATS:
                raise ValueError(f"Unsupported PLY format line {raw!r} in {filename!r}.")
            fmt = tokens[1]
        elif keyword == "element":
            if len(tokens) != 3:
                raise ValueError(f"Malformed PLY element line {raw!r} in {filename!r}.")
            try:
                count = int(tokens[2])
            except ValueError as exc:
                raise ValueError(f"Invalid PLY element count {tokens[2]!r} in {filename!r}.") from exc
            if count < 0:
                raise ValueError(f"PLY element count must be non-negative in {filename!r}. Got {count}.")
            elements.append(_PlyElement(tokens[1], count, []))
        elif keyword == "property":
            if not elements:
                raise ValueError(f"PLY property before any element in {filename!r}.")
            if len(tokens) == 3 and tokens[1] in _PLY_SCALAR_TYPES:
                elements[-1].properties.append((tokens[2], tokens[1]))
            elif (
                len(tokens) == 5
                and tokens[1] == "list"
                and tokens[2] in _PLY_SCALAR_TYPES
                and tokens[3] in _PLY_SCALAR_TYPES
            ):
                elements[-1].properties.append((tokens[4], None))
            else:
                raise ValueError(f"Malformed PLY property line {raw!r} in {filename!r}.")
        else:
            raise ValueError(f"Unknown PLY header keyword {keyword!r} in {filename!r}.")

    if fmt is None:
        raise ValueError(f"PLY header in {filename!r} has no 'format' line.")
    return _PlyHeader(fmt, elements)


def _ply_vertex_layout(header: _PlyHeader, filename: str) -> Tuple[int, _PlyElement, Tuple[int, int, int]]:
    """Locate the ``vertex`` element and the column indices of ``x``, ``y`` and ``z``."""
    for index, element in enumerate(header.elements):
        if element.name == "vertex":
            for name, scalar_type in element.properties:
                if scalar_type is None:
                    # An ASCII list spans ``1 + n`` tokens and a binary one an unknowable number of
                    # bytes, so a list among the vertex properties makes every column after it
                    # unlocatable. Only elements that follow the vertices may carry one.
                    raise ValueError(
                        f"PLY vertex element in {filename!r} has a list property {name!r}; "
                        "list properties are only supported in elements that follow the vertices."
                    )
            names = [name for name, _ in element.properties]
            try:
                xyz = (names.index("x"), names.index("y"), names.index("z"))
            except ValueError as exc:
                raise ValueError(
                    f"PLY vertex element in {filename!r} must declare x, y and z properties. Got {names}."
                ) from exc
            return index, element, xyz
    raise ValueError(f"PLY file {filename!r} declares no 'vertex' element.")


def _warn_header_size(header_size: Optional[int]) -> None:
    if header_size is not None:
        warnings.warn(
            "`header_size` is ignored since kornia 0.9.0: the PLY header is parsed up to `end_header`. "
            "The argument will be removed in a future release.",
            DeprecationWarning,
            stacklevel=3,
        )


def _check_ply_filename(filename: str) -> None:
    if not (isinstance(filename, str) and filename.lower().endswith(".ply")):
        raise TypeError(f"Input filename must be a str or os.PathLike[str] with the .ply extension. Got {filename!r}")
    if not os.path.isfile(filename):
        raise ValueError("Input filename is not an existing file.")


def load_pointcloud_ply(filename: str | os.PathLike[str], header_size: Optional[int] = None) -> torch.Tensor:
    r"""Read the x, y, z vertices of an ASCII PLY file into a float32 ``(N, 3)`` tensor.

    ``points = load_pointcloud_ply(filename)`` reads an ASCII PLY file and returns the ``x``, ``y`` and ``z`` of its
    ``N`` vertices as a float32 ``(N, 3)`` tensor on the CPU, so a float64 cloud written by
    :func:`save_pointcloud_ply` comes back rounded to float32. For a binary PLY file, use
    :func:`load_pointcloud_ply_binary` instead.

    The header is parsed up to ``end_header``; the number of points comes from the
    ``element vertex N`` declaration and the coordinates from the ``x``, ``y`` and ``z``
    properties, whatever other properties (normals, colours) or elements (faces) the file carries.
    A ``list`` property among the vertex properties is rejected, since it spans a variable number of
    tokens and would shift every column after it. See the Convention block on :func:`save_pointcloud_ply`.

    Args:
        filename: path of an existing ``.ply`` file, a ``str`` or ``os.PathLike[str]``.
        header_size: deprecated and ignored, with a ``DeprecationWarning`` when given; the header is parsed instead
            of skipped.

    Return:
        tensor containing the loaded points with shape ``(N, 3)`` in ``float32``, where
        :math:`N` is the declared vertex count.

    Raises:
        TypeError: if ``filename`` is not a ``str`` or ``os.PathLike[str]`` ending in ``.ply``.
        ValueError: if the file does not exist or does not start with a well-formed PLY header; if it is a binary
            PLY file; if it has no ``vertex`` element, no ``x``, ``y`` or ``z`` vertex property, or a ``list`` vertex
            property; or if the payload ends early, a vertex has fewer values than declared properties, or a
            coordinate is not a number.

    See Also:
        - :func:`save_pointcloud_ply`: writes the ASCII files this function reads.
        - :func:`load_pointcloud_ply_binary`: reads a binary PLY file, little- or big-endian.
    """
    filename = os.fspath(filename)
    _check_ply_filename(filename)
    _warn_header_size(header_size)

    with open(filename, "rb") as f:
        header = _read_ply_header(f, filename)
        if header.format != "ascii":
            raise ValueError(
                f"{filename!r} is a {header.format} PLY file; use `load_pointcloud_ply_binary` to read it."
            )
        vertex_index, vertex, xyz = _ply_vertex_layout(header, filename)

        # In ASCII PLY every element instance is one line, list properties included.
        for element in header.elements[:vertex_index]:
            for _ in range(element.count):
                if not f.readline():
                    raise ValueError(f"PLY file {filename!r} ends inside element {element.name!r}.")

        num_columns = len(vertex.properties)
        # The indices are hoisted and the three conversions written out: a generator expression here
        # costs ~30% of the read time on a 200k-point file.
        index_x, index_y, index_z = xyz
        points: List[float] = []
        for row in range(vertex.count):
            line = f.readline()
            if not line:
                raise ValueError(
                    f"PLY file {filename!r} declares {vertex.count} vertices but the payload ends after {row} of them."
                )
            tokens = line.split()
            if len(tokens) < num_columns:
                raise ValueError(
                    f"PLY file {filename!r} declares {vertex.count} vertices with {num_columns} properties, "
                    f"but vertex {row} has {len(tokens)} values."
                )
            try:
                points.extend((float(tokens[index_x]), float(tokens[index_y]), float(tokens[index_z])))
            except ValueError as exc:
                raise ValueError(f"PLY file {filename!r} has a non-numeric coordinate on vertex {row}: {exc}.") from exc

    return torch.tensor(points, dtype=torch.float32).reshape(-1, 3)


def load_pointcloud_ply_binary(filename: str | os.PathLike[str], header_size: Optional[int] = None) -> torch.Tensor:
    r"""Read the x, y, z vertices of a big- or little-endian binary PLY file as ``(N, 3)``.

    ``points = load_pointcloud_ply_binary(filename)`` reads a ``binary_little_endian`` or ``binary_big_endian`` PLY
    file and returns the ``x``, ``y`` and ``z`` of its ``N`` vertices as a float32 ``(N, 3)`` tensor on the CPU, so a
    float64 cloud written by :func:`save_pointcloud_ply_binary` comes back rounded to float32. For an ASCII PLY file,
    use :func:`load_pointcloud_ply` instead.

    The header is parsed up to ``end_header``; the number of points comes from the
    ``element vertex N`` declaration and the coordinates from the ``x``, ``y`` and ``z``
    properties, whatever their scalar type. Extra vertex properties (normals, colours) are skipped,
    and so are elements that follow the vertices (faces). Both little- and big-endian payloads are
    read. A ``list`` property is only supported in elements that *follow* the vertices. Among the
    vertex properties, or in an element that precedes them, its byte length is unknown without
    parsing it, so such a file is rejected. See the Convention block on :func:`save_pointcloud_ply`.

    Args:
        filename: path of an existing ``.ply`` file, a ``str`` or ``os.PathLike[str]``.
        header_size: deprecated and ignored, with a ``DeprecationWarning`` when given; the header is parsed instead
            of skipped.

    Return:
        tensor containing the loaded points with shape ``(N, 3)`` in ``float32``, where
        :math:`N` is the declared vertex count.

    Raises:
        TypeError: if ``filename`` is not a ``str`` or ``os.PathLike[str]`` ending in ``.ply``.
        ValueError: if the file does not exist or does not start with a well-formed PLY header; if it is an ASCII
            PLY file; if it has no ``vertex`` element or no ``x``, ``y`` or ``z`` vertex property; if a ``list``
            property is among the vertex properties or in an element before them; or if fewer bytes follow the
            header than the vertices need.

    See Also:
        - :func:`save_pointcloud_ply_binary`: writes the little-endian files this function reads.
        - :func:`load_pointcloud_ply`: reads an ASCII PLY file.
    """
    filename = os.fspath(filename)
    _check_ply_filename(filename)
    _warn_header_size(header_size)

    with open(filename, "rb") as f:
        header = _read_ply_header(f, filename)
        if header.format == "ascii":
            raise ValueError(f"{filename!r} is an ASCII PLY file; use `load_pointcloud_ply` to read it.")
        endian = "<" if header.format == "binary_little_endian" else ">"
        vertex_index, vertex, xyz = _ply_vertex_layout(header, filename)

        def element_struct(element: _PlyElement) -> str:
            codes = []
            for name, scalar_type in element.properties:
                if scalar_type is None:
                    raise ValueError(
                        f"PLY element {element.name!r} in {filename!r} has a list property {name!r}; "
                        "list properties are only supported after the vertex element."
                    )
                codes.append(_PLY_SCALAR_TYPES[scalar_type][0])
            return endian + "".join(codes)

        for element in header.elements[:vertex_index]:
            if element.count > 0:
                f.seek(element.count * struct.calcsize(element_struct(element)), os.SEEK_CUR)

        vertex_format = element_struct(vertex)
        stride = struct.calcsize(vertex_format)
        expected = vertex.count * stride
        data = f.read(expected)
        if len(data) != expected:
            raise ValueError(
                f"PLY file {filename!r} declares {vertex.count} vertices ({expected} bytes) "
                f"but only {len(data)} bytes follow the header."
            )

    if vertex.count == 0:
        return torch.empty((0, 3), dtype=torch.float32)

    scalar_types = {scalar_type for _, scalar_type in vertex.properties}
    if len(scalar_types) == 1:
        code, dtype = _PLY_SCALAR_TYPES[next(iter(scalar_types))]
        if dtype is not None:
            # Homogeneous layout: one bulk copy, then column-select x, y, z.
            values = array.array(code, data)
            if (endian == "<") != (sys.byteorder == "little"):
                values.byteswap()
            table = torch.frombuffer(values, dtype=dtype).reshape(vertex.count, len(vertex.properties))
            # Advanced indexing already copies out of the buffer, so the result never aliases ``values``.
            return table[:, list(xyz)].to(torch.float32)

    # Mixed layout (or an unsigned type torch cannot hold): unpack row by row.
    rows = struct.iter_unpack(vertex_format, data)
    return torch.tensor([[row[i] for i in xyz] for row in rows], dtype=torch.float32)
