#!/usr/bin/env python3
"""Validate the real-robot navigation interfaces before sending a goal."""

import sys
import time

import rclpy
from nav_msgs.msg import OccupancyGrid, Odometry
from rclpy.duration import Duration
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from rclpy.time import Time
from sensor_msgs.msg import LaserScan
from tf2_ros import Buffer, TransformListener


class PreflightCheck(Node):
    def __init__(self):
        super().__init__("hybrid_astar_preflight")
        self.declare_parameter("expected_map_width", 5.0)
        self.declare_parameter("expected_map_height", 5.0)
        self.declare_parameter("map_size_tolerance", 0.15)
        self.declare_parameter("timeout_sec", 10.0)
        self.declare_parameter("require_map", True)
        self.declare_parameter("require_scan", True)
        self.declare_parameter("require_tf", True)
        self.declare_parameter("require_odom_tf", False)
        self.declare_parameter("require_base_tf", True)
        self.declare_parameter("map_frame", "map")
        self.declare_parameter("odom_frame", "odom")
        self.declare_parameter("base_frame", "base_footprint")
        self.declare_parameter("map_topic", "/map")
        self.declare_parameter("odom_topic", "/odom_combined")
        self.declare_parameter("scan_topic", "/scan")
        self.declare_parameter("cmd_vel_topic", "/planner_cmd_vel")
        self.declare_parameter("expected_scan_frame", "laser_link")
        self.declare_parameter("tf_wait_sec", 3.0)
        self.declare_parameter("drive_command_topic", "/ackermann_cmd")

        self.require_map = bool(self.get_parameter("require_map").value)
        self.require_scan = bool(self.get_parameter("require_scan").value)
        self.require_tf = bool(self.get_parameter("require_tf").value)
        self.require_odom_tf = bool(self.get_parameter("require_odom_tf").value)
        self.require_base_tf = bool(self.get_parameter("require_base_tf").value)
        self.map_topic = str(self.get_parameter("map_topic").value)
        self.odom_topic = str(self.get_parameter("odom_topic").value)
        self.scan_topic = str(self.get_parameter("scan_topic").value)
        self.cmd_vel_topic = str(self.get_parameter("cmd_vel_topic").value)

        map_qos = QoSProfile(
            depth=1,
            reliability=ReliabilityPolicy.RELIABLE,
            durability=DurabilityPolicy.TRANSIENT_LOCAL,
        )

        self.map_msg = None
        self.odom_msg = None
        self.scan_msg = None
        self.create_subscription(OccupancyGrid, self.map_topic, self._map_cb, map_qos)
        self.create_subscription(Odometry, self.odom_topic, self._odom_cb, 10)
        self.create_subscription(LaserScan, self.scan_topic, self._scan_cb, 10)
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

    def _map_cb(self, msg):
        self.map_msg = msg

    def _odom_cb(self, msg):
        self.odom_msg = msg

    def _scan_cb(self, msg):
        self.scan_msg = msg

    def wait_for_inputs(self):
        deadline = time.monotonic() + float(self.get_parameter("timeout_sec").value)
        while rclpy.ok() and time.monotonic() < deadline:
            rclpy.spin_once(self, timeout_sec=0.1)
            map_ready = self.map_msg is not None or not self.require_map
            odom_ready = self.odom_msg is not None
            scan_ready = self.scan_msg is not None or not self.require_scan
            if map_ready and odom_ready and scan_ready:
                return

    def evaluate(self):
        errors = []
        if self.map_msg is None:
            if self.require_map:
                errors.append("no %s OccupancyGrid received" % self.map_topic)
        else:
            map_frame = str(self.get_parameter("map_frame").value)
            if (
                self.map_msg.header.frame_id
                and self.map_msg.header.frame_id != map_frame
            ):
                errors.append(
                    "/map frame is %s, expected %s"
                    % (self.map_msg.header.frame_id, map_frame)
                )
            width = self.map_msg.info.width * self.map_msg.info.resolution
            height = self.map_msg.info.height * self.map_msg.info.resolution
            expected_width = float(self.get_parameter("expected_map_width").value)
            expected_height = float(self.get_parameter("expected_map_height").value)
            tolerance = float(self.get_parameter("map_size_tolerance").value)
            self.get_logger().info(
                f"map size {width:.3f} x {height:.3f} m, "
                f"resolution {self.map_msg.info.resolution:.3f} m"
            )
            if (
                abs(width - expected_width) > tolerance
                or abs(height - expected_height) > tolerance
            ):
                errors.append(
                    "map is not the expected %.2f x %.2f m (tolerance %.2f m)"
                    % (expected_width, expected_height, tolerance)
                )

        if self.odom_msg is None:
            errors.append("no %s received" % self.odom_topic)
        if self.scan_msg is None:
            if self.require_scan:
                errors.append("no %s received" % self.scan_topic)
        elif not self.scan_msg.header.frame_id:
            errors.append("%s frame_id is empty" % self.scan_topic)
        else:
            expected_scan_frame = str(
                self.get_parameter("expected_scan_frame").value
            )
            if (
                expected_scan_frame
                and self.scan_msg.header.frame_id != expected_scan_frame
            ):
                errors.append(
                    "%s frame is %s, expected %s"
                    % (self.scan_topic, self.scan_msg.header.frame_id, expected_scan_frame)
                )

        map_frame = str(self.get_parameter("map_frame").value)
        odom_frame = str(self.get_parameter("odom_frame").value)
        base_frame = str(self.get_parameter("base_frame").value)
        if self.require_tf:
            if self.require_odom_tf:
                if not self._wait_for_tf(map_frame, odom_frame):
                    errors.append("TF unavailable: %s -> %s" % (map_frame, odom_frame))

            if self.require_base_tf and not self._wait_for_tf(map_frame, base_frame):
                errors.append("TF unavailable: %s -> %s" % (map_frame, base_frame))
            elif (
                self.require_base_tf
                and self.scan_msg is not None
                and self.scan_msg.header.frame_id
            ):
                scan_frame = self.scan_msg.header.frame_id
                if not self._wait_for_tf(base_frame, scan_frame):
                    errors.append("TF unavailable: %s -> %s" % (base_frame, scan_frame))
        elif self.require_scan and self.scan_msg is not None and self.scan_msg.header.frame_id:
            scan_frame = self.scan_msg.header.frame_id
            self.get_logger().info("scan frame check only: %s" % scan_frame)

        publisher_deadline = time.monotonic() + 1.5
        cmd_publishers = self.get_publishers_info_by_topic(self.cmd_vel_topic)
        while not cmd_publishers and time.monotonic() < publisher_deadline:
            rclpy.spin_once(self, timeout_sec=0.1)
            cmd_publishers = self.get_publishers_info_by_topic(self.cmd_vel_topic)
        if len(cmd_publishers) != 1:
            names = [
                "%s/%s" % (info.node_namespace, info.node_name)
                for info in cmd_publishers
            ]
            errors.append(
                "expected exactly one %s publisher, found %d: %s"
                % (self.cmd_vel_topic, len(cmd_publishers), ", ".join(names) or "none")
            )

        drive_topic = str(self.get_parameter("drive_command_topic").value)
        drive_publishers = self.get_publishers_info_by_topic(drive_topic)
        drive_subscribers = self.get_subscriptions_info_by_topic(drive_topic)
        if len(drive_publishers) != 1:
            names = [
                "%s/%s" % (info.node_namespace, info.node_name)
                for info in drive_publishers
            ]
            errors.append(
                "expected exactly one %s publisher, found %d: %s"
                % (drive_topic, len(drive_publishers), ", ".join(names) or "none")
            )
        if len(drive_subscribers) != 1:
            names = [
                "%s/%s" % (info.node_namespace, info.node_name)
                for info in drive_subscribers
            ]
            errors.append(
                "expected exactly one %s subscriber, found %d: %s"
                % (drive_topic, len(drive_subscribers), ", ".join(names) or "none")
            )

        for error in errors:
            self.get_logger().error(error)
        if errors:
            return False

        self.get_logger().info("preflight passed: navigation stack is ready for a goal")
        return True

    def _wait_for_tf(self, target_frame, source_frame):
        tf_deadline = time.monotonic() + max(
            0.5, float(self.get_parameter("tf_wait_sec").value)
        )
        while rclpy.ok() and time.monotonic() < tf_deadline:
            if self.tf_buffer.can_transform(
                target_frame,
                source_frame,
                Time(),
                timeout=Duration(seconds=0.2),
            ):
                return True
            rclpy.spin_once(self, timeout_sec=0.05)
        return False


def main():
    rclpy.init()
    node = PreflightCheck()
    success = False
    try:
        node.wait_for_inputs()
        success = node.evaluate()
    finally:
        node.destroy_node()
        rclpy.shutdown()
    if not success:
        sys.exit(1)


if __name__ == "__main__":
    main()
