"""
frontier_detector_node.py — Detect exploration frontiers from the merged fleet map.

Subscribes:
  /fleet/merged_map             (OccupancyGrid, TRANSIENT_LOCAL) — merged fleet map

Publishes:
  /fleet/exploration_frontiers_raw  (FrontierArray)  — raw detections sent to manager
  /fleet/exploration_frontiers      (FrontierArray)  — also publish locally (for viz)

Parameters:
  robot_id            str    "saltybot_1"
  detection_hz        float  1.0
  min_frontier_cells  int    10     (filter noise clusters smaller than this)
  robot_pose_topic    str    ""     (optional geometry_msgs/PoseStamped for scoring)
"""

import rclpy
from rclpy.node import Node
from rclpy.qos import (
    QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
)
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PoseStamped

from saltybot_fleet_msgs.msg import FrontierArray

from .frontier_detector import detect_frontiers


_TRANSIENT_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
)


class FrontierDetectorNode(Node):

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

        self.declare_parameter('robot_id',           'saltybot_1')
        self.declare_parameter('detection_hz',       1.0)
        self.declare_parameter('min_frontier_cells', 10)
        self.declare_parameter('robot_pose_topic',   '')

        self._robot_id        = self.get_parameter('robot_id').value
        hz                    = self.get_parameter('detection_hz').value
        self._min_cells       = self.get_parameter('min_frontier_cells').value
        pose_topic            = self.get_parameter('robot_pose_topic').value

        self._grid:       OccupancyGrid | None = None
        self._robot_xy:   tuple | None         = None
        self._detect_seq: int                  = 0

        self.create_subscription(OccupancyGrid, '/fleet/merged_map', self._on_map, _TRANSIENT_QOS)
        if pose_topic:
            self.create_subscription(PoseStamped, pose_topic, self._on_pose, 10)

        self._raw_pub   = self.create_publisher(FrontierArray, '/fleet/exploration_frontiers_raw', 10)
        self._local_pub = self.create_publisher(FrontierArray, '/fleet/exploration_frontiers',     10)

        self.create_timer(1.0 / hz, self._detect_tick)
        self.get_logger().info(f'[{self._robot_id}] frontier_detector ready @ {hz:.1f} Hz')

    def _on_map(self, msg: OccupancyGrid) -> None:
        self._grid = msg

    def _on_pose(self, msg: PoseStamped) -> None:
        p = msg.pose.position
        self._robot_xy = (p.x, p.y)

    def _detect_tick(self) -> None:
        if self._grid is None:
            return
        self._detect_seq += 1
        frontiers = detect_frontiers(
            self._grid,
            robot_xy=self._robot_xy,
            min_area_cells=self._min_cells,
            robot_id=self._robot_id,
            seq=self._detect_seq,
        )
        if not frontiers:
            return
        fa = FrontierArray()
        fa.stamp        = self.get_clock().now().to_msg()
        fa.source_robot = self._robot_id
        fa.frontiers    = frontiers
        self._raw_pub.publish(fa)
        self._local_pub.publish(fa)
        self.get_logger().debug(
            f'[{self._robot_id}] detected {len(frontiers)} frontiers'
        )


def main(args=None):
    rclpy.init(args=args)
    node = FrontierDetectorNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
