#!/usr/bin/env python3
"""Check whether LaserScan remains usable despite driver console warnings."""

import math
import sys
import time

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan


class LidarHealthCheck(Node):
    def __init__(self):
        super().__init__("planner_lidar_health_check")
        self.declare_parameter("scan_topic", "/scan")
        self.declare_parameter("duration_sec", 10.0)
        self.declare_parameter("min_rate_hz", 5.0)
        self.declare_parameter("min_valid_fraction", 0.02)
        self.scan_topic = str(self.get_parameter("scan_topic").value)
        self.duration_sec = max(2.0, float(self.get_parameter("duration_sec").value))
        self.min_rate_hz = max(0.1, float(self.get_parameter("min_rate_hz").value))
        self.min_valid_fraction = max(
            0.0, min(1.0, float(self.get_parameter("min_valid_fraction").value))
        )
        self.start_monotonic = time.monotonic()
        self.receipt_times = []
        self.valid_fractions = []
        self.frame_ids = set()
        self.subscription = self.create_subscription(
            LaserScan, self.scan_topic, self._scan_cb, 20
        )

    def _scan_cb(self, msg):
        self.receipt_times.append(time.monotonic())
        self.frame_ids.add(msg.header.frame_id)
        if msg.ranges:
            valid = sum(
                1
                for value in msg.ranges
                if math.isfinite(value) and msg.range_min <= value <= msg.range_max
            )
            self.valid_fractions.append(valid / len(msg.ranges))

    def complete(self):
        return time.monotonic() - self.start_monotonic >= self.duration_sec

    def evaluate(self):
        elapsed = time.monotonic() - self.start_monotonic
        if len(self.receipt_times) >= 2:
            span = self.receipt_times[-1] - self.receipt_times[0]
            rate = (len(self.receipt_times) - 1) / span if span > 0.0 else 0.0
        else:
            rate = 0.0
        valid_mean = (
            sum(self.valid_fractions) / len(self.valid_fractions)
            if self.valid_fractions
            else 0.0
        )
        errors = []
        if not self.receipt_times:
            errors.append(f"no {self.scan_topic} received in {elapsed:.1f} s")
        if rate < self.min_rate_hz:
            errors.append(f"scan rate {rate:.2f} Hz is below {self.min_rate_hz:.2f} Hz")
        if valid_mean < self.min_valid_fraction:
            errors.append(
                f"valid range fraction {valid_mean:.3f} is below "
                f"{self.min_valid_fraction:.3f}"
            )
        if not self.frame_ids or "" in self.frame_ids:
            errors.append("LaserScan frame_id is empty")

        self.get_logger().info(
            f"scans={len(self.receipt_times)} rate={rate:.2f} Hz "
            f"valid_fraction={valid_mean:.3f} frames={sorted(self.frame_ids)}"
        )
        for error in errors:
            self.get_logger().error(error)
        if not errors:
            self.get_logger().info("PASS: lidar data is usable by AMCL")
        return not errors


def main():
    rclpy.init()
    node = LidarHealthCheck()
    try:
        while rclpy.ok() and not node.complete():
            rclpy.spin_once(node, timeout_sec=0.1)
        success = node.evaluate()
    finally:
        node.destroy_node()
        rclpy.shutdown()
    sys.exit(0 if success else 1)


if __name__ == "__main__":
    main()
