#!/usr/bin/env python3
"""Geofence boundary enforcer for SaltyBot.

Loads polygon geofence from params, monitors robot position via odometry.
Publishes Bool on /saltybot/geofence_breach when exiting boundary.
Optionally zeros cmd_vel to enforce boundary.

Subscribed topics:
  /odom (nav_msgs/Odometry) - Robot position and orientation

Published topics:
  /saltybot/geofence_breach (std_msgs/Bool) - Outside boundary flag
"""

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from std_msgs.msg import Bool
from typing import List, Tuple


class GeofenceNode(Node):
    """ROS2 node for geofence boundary enforcement."""

    def __init__(self):
        super().__init__("geofence")

        # Parameters
        self.declare_parameter("geofence_vertices", [])
        self.declare_parameter("enforce_boundary", False)
        self.declare_parameter("margin", 0.0)

        vertices = self.get_parameter("geofence_vertices").value
        self.enforce_boundary = self.get_parameter("enforce_boundary").value
        self.margin = self.get_parameter("margin").value

        # Parse vertices from flat list [x1,y1,x2,y2,...]
        self.geofence_vertices = self._parse_vertices(vertices)

        # State tracking
        self.robot_x = 0.0
        self.robot_y = 0.0
        self.inside_geofence = True
        self.breach_published = False

        # Subscription to odometry
        self.sub_odom = self.create_subscription(
            Odometry, "/odom", self._on_odometry, 10
        )

        # Publisher for breach status
        self.pub_breach = self.create_publisher(Bool, "/saltybot/geofence_breach", 10)

        self.get_logger().info(
            f"Geofence enforcer initialized with {len(self.geofence_vertices)} vertices. "
            f"Enforce: {self.enforce_boundary}, Margin: {self.margin}m"
        )

        if len(self.geofence_vertices) > 0:
            self.get_logger().info(f"Geofence vertices: {self.geofence_vertices}")

    def _parse_vertices(self, flat_list: List[float]) -> List[Tuple[float, float]]:
        """Parse flat list [x1,y1,x2,y2,...] into vertex tuples."""
        if len(flat_list) < 6:  # Need at least 3 vertices (6 values)
            self.get_logger().warn("Geofence needs at least 3 vertices (6 values)")
            return []

        vertices = []
        for i in range(0, len(flat_list) - 1, 2):
            vertices.append((flat_list[i], flat_list[i + 1]))

        return vertices

    def _on_odometry(self, msg: Odometry) -> None:
        """Process odometry and check geofence boundary."""
        if len(self.geofence_vertices) == 0:
            # No geofence defined
            self.inside_geofence = True
            return

        # Extract robot position
        self.robot_x = msg.pose.pose.position.x
        self.robot_y = msg.pose.pose.position.y

        # Check if inside geofence
        self.inside_geofence = self._point_in_polygon(
            (self.robot_x, self.robot_y), self.geofence_vertices
        )

        # Publish breach status
        breach = not self.inside_geofence
        if breach and not self.breach_published:
            self.get_logger().warn(
                f"GEOFENCE BREACH! Robot at ({self.robot_x:.2f}, {self.robot_y:.2f})"
            )
            self.breach_published = True
        elif not breach and self.breach_published:
            self.get_logger().info(
                f"Robot re-entered geofence at ({self.robot_x:.2f}, {self.robot_y:.2f})"
            )
            self.breach_published = False

        msg_breach = Bool(data=breach)
        self.pub_breach.publish(msg_breach)

    def _point_in_polygon(self, point: Tuple[float, float], vertices: List[Tuple[float, float]]) -> bool:
        """Ray casting algorithm for point-in-polygon test."""
        x, y = point
        n = len(vertices)
        inside = False

        p1x, p1y = vertices[0]
        for i in range(1, n + 1):
            p2x, p2y = vertices[i % n]

            # Check if ray crosses edge
            if y > min(p1y, p2y):
                if y <= max(p1y, p2y):
                    if x <= max(p1x, p2x):
                        if p1y != p2y:
                            xinters = (y - p1y) * (p2x - p1x) / (p2y - p1y) + p1x
                        if p1x == p2x or x <= xinters:
                            inside = not inside

            p1x, p1y = p2x, p2y

        return inside


def main(args=None):
    rclpy.init(args=args)
    node = GeofenceNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
