from __future__ import annotations

from dataclasses import dataclass

import numpy as np
import open3d as o3d


@dataclass
class PlaneResult:
    model: tuple[float, float, float, float]
    inliers: list[int]
    inlier_ratio: float
    normal_angle_deg: float
    rmse: float
    pointcloud: o3d.geometry.PointCloud


class PlaneEstimator:
    """Generate noisy planes and estimate planes using Open3D RANSAC."""

    def create_square_plane_with_noise(
        self,
        size_mm: float = 1000.0,
        noise_std_mm: float = 0.5,
        num_points: int = 10000,
    ) -> o3d.geometry.PointCloud:
        half_size_m = size_mm / 2000.0
        noise_std_m = noise_std_mm / 1000.0
        xy = np.random.uniform(-half_size_m, half_size_m, size=(num_points, 2))
        z = np.random.normal(0, noise_std_m, size=(num_points, 1))
        points = np.hstack([xy, z])
        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(points)
        return pcd

    def estimate(
        self,
        pointcloud: o3d.geometry.PointCloud,
        threshold_mm: float = 0.5,
        max_iterations: int = 2000,
    ) -> PlaneResult:
        threshold_m = threshold_mm / 1000.0
        plane_model, inliers = pointcloud.segment_plane(
            distance_threshold=threshold_m,
            ransac_n=3,
            num_iterations=max_iterations,
        )

        a, b, c, d = plane_model
        normal = np.array([a, b, c], dtype=np.float64)
        normal = normal / (np.linalg.norm(normal) + 1e-12)
        ideal = np.array([0.0, 0.0, 1.0])
        angle = float(np.degrees(np.arccos(np.clip(abs(np.dot(normal, ideal)), -1.0, 1.0))))

        points = np.asarray(pointcloud.points)
        inlier_points = points[inliers]
        distances = np.abs(np.dot(inlier_points, np.array([a, b, c])) + d)
        rmse = float(np.sqrt(np.mean(distances ** 2))) if len(distances) else 0.0
        ratio = len(inliers) / len(points) * 100 if len(points) else 0.0

        return PlaneResult(tuple(float(x) for x in plane_model), list(inliers), ratio, angle, rmse, pointcloud)