#!/usr/bin/python3
# Copyright 2026 Alexandros Filotheou
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

import os
from typing import List

import numpy as np
# Open3D must be installed in your environment (pip install open3d)
import open3d as o3d
import rclpy
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

from sensor_msgs.msg import PointCloud2
import sensor_msgs_py.point_cloud2 as pc2


def cloud_to_numpy(msg: PointCloud2) -> np.ndarray:
    """Convert ROS2 PointCloud2 to (N,3) numpy array."""
    pts = np.asarray([
        (x, y, z)
        for x, y, z in pc2.read_points(msg, field_names=('x', 'y', 'z'), skip_nans=True)
    ], dtype=np.float64)
    return pts


def numpy_to_o3d_pointcloud(points: np.ndarray) -> o3d.geometry.PointCloud:
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    return pcd


def preprocess_pointcloud(
    pcd: o3d.geometry.PointCloud,
    voxel_downsample_size: float,
    remove_statistical_outliers: bool,
    nso_nb_neighbors: int,
    nso_std_ratio: float,
) -> o3d.geometry.PointCloud:
    if voxel_downsample_size and voxel_downsample_size > 0.0:
        pcd = pcd.voxel_down_sample(voxel_downsample_size)

    if remove_statistical_outliers:
        pcd, _ = pcd.remove_statistical_outlier(
            nb_neighbors=nso_nb_neighbors, std_ratio=nso_std_ratio
        )
    return pcd


def estimate_normals(
    pcd: o3d.geometry.PointCloud, normal_radius: float, normal_max_nn: int
):
    pcd.estimate_normals(
        search_param=o3d.geometry.KDTreeSearchParamHybrid(
            radius=normal_radius, max_nn=normal_max_nn
        )
    )
    pcd.orient_normals_consistent_tangent_plane(10)


def poisson_reconstruction(
    pcd: o3d.geometry.PointCloud, depth: int, density_quantile: float
) -> o3d.geometry.TriangleMesh:
    mesh, densities = o3d.geometry.TriangleMesh.create_from_point_cloud_poisson(
        pcd, depth=depth
    )
    if density_quantile is not None and 0.0 < density_quantile < 1.0:
        densities = np.asarray(densities)
        mask = densities < np.quantile(densities, density_quantile)
        mesh.remove_vertices_by_mask(mask)
    mesh.remove_degenerate_triangles()
    mesh.remove_duplicated_triangles()
    mesh.remove_duplicated_vertices()
    mesh.remove_non_manifold_edges()
    return mesh


def bpa_reconstruction(
    pcd: o3d.geometry.PointCloud, radii: List[float], tri_angle_deg: float
) -> o3d.geometry.TriangleMesh:
    # Open3D BPA uses radii list; ensure ascending and >0
    radii = sorted([r for r in radii if r > 0.0])
    if not radii:
        raise ValueError('bpa_radii must contain positive values')
    mesh = o3d.geometry.TriangleMesh.create_from_point_cloud_ball_pivoting(
        pcd, o3d.utility.DoubleVector(radii)
    )
    # Optional: simplify very sharp triangles if angle threshold provided
    if tri_angle_deg is not None:
        mesh = mesh.filter_smooth_taubin(number_of_iterations=1)
    mesh.remove_degenerate_triangles()
    mesh.remove_duplicated_triangles()
    mesh.remove_duplicated_vertices()
    mesh.remove_non_manifold_edges()
    return mesh


def save_mesh(mesh: o3d.geometry.TriangleMesh, out_dir: str, basename: str, fmt: str) -> str:
    os.makedirs(out_dir, exist_ok=True)
    fmt = fmt.lower()
    if fmt not in ('obj', 'ply'):
        raise ValueError('output_format must be "obj" or "ply"')
    path = os.path.join(out_dir, f'{basename}.{fmt}')
    o3d.io.write_triangle_mesh(path, mesh)
    return path


class PointCloudToMeshNode(Node):

    def __init__(self):
        super().__init__('pointcloud_to_ply_node')

        # Parameters (declare with defaults; override via YAML)
        self.declare_parameter('pointcloud_topic', '/rc_viscore/points2')
        self.declare_parameter('output_path', '/tmp/mesh_output')
        self.declare_parameter('output_basename', 'mesh')
        self.declare_parameter('output_format', 'obj')

        self.declare_parameter('voxel_downsample_size', 0.0)
        self.declare_parameter('remove_statistical_outliers', True)
        self.declare_parameter('nso_nb_neighbors', 20)
        self.declare_parameter('nso_std_ratio', 2.0)

        self.declare_parameter('normal_radius', 0.05)
        self.declare_parameter('normal_max_nn', 30)

        self.declare_parameter('reconstruction_method', 'poisson')
        self.declare_parameter('poisson_depth', 9)
        self.declare_parameter('poisson_density_quantile', 0.05)
        self.declare_parameter('bpa_radii', [0.02, 0.04, 0.08])
        self.declare_parameter('bpa_triangle_angle', 30.0)

        self.declare_parameter('shutdown_after_save', True)
        self.declare_parameter('log_level_debug', False)

        topic = self.get_parameter('pointcloud_topic').get_parameter_value().string_value

        # Should match the topic's qos profile
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )

        self.subscription = self.create_subscription(
            PointCloud2, topic, self._callback, qos
        )
        self._done = False

        self.get_logger().info(f'Subscribed to {topic}')

    def _callback(self, msg: PointCloud2):
        if self._done:
            return
        self._done = True

        try:
            self.get_logger().info('Received point cloud; converting to numpy...')
            points = cloud_to_numpy(msg)
            self.get_logger().info(f'Point count: {points.shape[0]}')

            pcd = numpy_to_o3d_pointcloud(points)

            # Preprocess
            pcd = preprocess_pointcloud(
                pcd=pcd,
                voxel_downsample_size=self.get_parameter('voxel_downsample_size').value,
                remove_statistical_outliers=self.get_parameter(
                    'remove_statistical_outliers'
                ).value,
                nso_nb_neighbors=self.get_parameter('nso_nb_neighbors').value,
                nso_std_ratio=self.get_parameter('nso_std_ratio').value,
            )

            # Normals
            estimate_normals(
                pcd,
                self.get_parameter('normal_radius').value,
                self.get_parameter('normal_max_nn').value,
            )

            # Reconstruction method
            method = self.get_parameter('reconstruction_method').value.lower()
            if method == 'poisson':
                depth = int(self.get_parameter('poisson_depth').value)
                dq = float(self.get_parameter('poisson_density_quantile').value)
                mesh = poisson_reconstruction(pcd, depth, dq)
                self.get_logger().info(f'Poisson reconstruction complete (depth={depth})')
            elif method == 'bpa':
                radii = list(self.get_parameter('bpa_radii').value)
                tri_angle = float(self.get_parameter('bpa_triangle_angle').value)
                mesh = bpa_reconstruction(pcd, radii, tri_angle)
                self.get_logger().info(f'BPA reconstruction complete (radii={radii})')
            else:
                raise ValueError('reconstruction_method must be "poisson" or "bpa"')

            out_dir = self.get_parameter('output_path').value
            basename = self.get_parameter('output_basename').value
            fmt = self.get_parameter('output_format').value
            out_path = save_mesh(mesh, out_dir, basename, fmt)
            self.get_logger().info(f'Saved mesh to: {out_path}')

        except Exception as e:
            self.get_logger().error(f'Failed to create/save mesh: {e}')
        finally:
            if self.get_parameter('shutdown_after_save').value:
                self.get_logger().info('Shutting down node.')
                rclpy.shutdown()


def main(args=None):
    rclpy.init(args=args)
    node = PointCloudToMeshNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()
