#!/usr/bin/env python3
"""四角 LiDAR + 车端标定球外部真值定位算法模板。"""

from __future__ import annotations

from dataclasses import dataclass, field
from typing import Sequence


@dataclass(frozen=True)
class Pose3DValue:
    """车间坐标系下的三维位姿。"""

    x_m: float = 0.0
    y_m: float = 0.0
    z_m: float = 0.0
    roll_rad: float = 0.0
    pitch_rad: float = 0.0
    yaw_rad: float = 0.0


@dataclass(frozen=True)
class LidarMountConfig:
    """单台固定 LiDAR 在车间坐标系下的静态配置。"""

    lidar_id: str
    topic: str
    frame_id: str
    workshop_pose: Pose3DValue
    enabled: bool = True


@dataclass(frozen=True)
class BallTargetConfig:
    """车端标定球在 base_link 坐标系下的安装配置。"""

    ball_id: str
    radius_m: float
    center_in_base_link: Pose3DValue
    required: bool = True


@dataclass(frozen=True)
class LidarFrame:
    """单帧 LiDAR 数据，点坐标保持在原始 LiDAR frame 下。"""

    lidar_id: str
    frame_id: str
    stamp_us: int
    points_xyz: Sequence[tuple[float, float, float]]
    point_count: int


@dataclass(frozen=True)
class BallObservation:
    """算法识别出的单个标定球观测。"""

    ball_id: str
    center_in_workshop: Pose3DValue
    residual_m: float = 0.0
    quality_score: float = 0.0
    source_lidar_ids: Sequence[str] = field(default_factory=tuple)


@dataclass(frozen=True)
class FourLidarBallLocalizationInput:
    """算法输入快照。"""

    frames: Sequence[LidarFrame]
    lidar_mounts: Sequence[LidarMountConfig]
    ball_targets: Sequence[BallTargetConfig]
    max_frame_age_ms: float
    publish_timestamp_us: int
    reference_source_name: str
    workcell_zone_id: str


@dataclass
class FourLidarBallLocalizationOutput:
    """算法输出结果，最终会被节点转换成 ExternalLocalizationTelemetry。"""

    pose_valid: bool = False
    workshop_pose: Pose3DValue = field(default_factory=Pose3DValue)
    position_stddev_m: float = 0.0
    yaw_stddev_rad: float = 0.0
    tracking_loss_ratio: float = 1.0
    time_sync_offset_ms: float = 0.0
    quality_score: float = 0.0
    observed_target_count: int = 0
    hardware_timestamp_us: int = 0
    observed_ball_targets: list[BallObservation] = field(default_factory=list)
    diagnostic_messages: list[str] = field(default_factory=list)


class FourLidarBallLocalizationAlgorithm:
    """算法实现者只需要替换这个类里的 solve 方法。"""

    def solve(self, data: FourLidarBallLocalizationInput) -> FourLidarBallLocalizationOutput:
        """根据四角 LiDAR 数据和标定球配置求解车辆 base_link 位姿。"""
        output = FourLidarBallLocalizationOutput()
        output.hardware_timestamp_us = data.publish_timestamp_us
        output.diagnostic_messages.append(
            "四角 LiDAR + 标定球定位算法尚未实现，请在 solve() 中填充真实求解逻辑。"
        )
        return output
