"""
geofence_node — GPS polygon safety boundary for outdoor navigation

Monitors /gps/fix and checks the robot's position against a configurable
polygon of GPS vertices. If the robot exits the fence OR a requested waypoint
lies outside the fence, this node:
  1. Publishes /outdoor/abort True → gps_waypoint_follower cancels Nav2
  2. Publishes /cmd_vel zero twist (emergency stop)
  3. Publishes /outdoor/fence_violation with details

The fence polygon is defined in geofence_vertices.yaml as a list of
{lat, lon} pairs forming a closed polygon (first = last not required).
Point-in-polygon test uses the ray-casting algorithm.

Topics subscribed:
  /gps/fix               sensor_msgs/NavSatFix
  /outdoor/waypoints     geometry_msgs/PoseArray  — pre-navigation check

Topics published:
  /outdoor/abort         std_msgs/Bool            — True when fence violated
  /outdoor/fence_status  std_msgs/String          — 'inside' | 'outside:<lat>,<lon>'
  /cmd_vel               geometry_msgs/Twist      — zero-velocity on violation

Services:
  /outdoor/reload_fence  std_srvs/Trigger         — re-read geofence_vertices.yaml
"""

import math
from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy

from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import PoseArray, Twist
from std_msgs.msg import Bool, String
from std_srvs.srv import Trigger


def _point_in_polygon(lat: float, lon: float, polygon: list) -> bool:
    """Ray-casting point-in-polygon test. polygon = [(lat, lon), ...]."""
    n = len(polygon)
    inside = False
    j = n - 1
    for i in range(n):
        xi, yi = polygon[i]
        xj, yj = polygon[j]
        if ((yi > lon) != (yj > lon)) and (lat < (xj - xi) * (lon - yi) / (yj - yi) + xi):
            inside = not inside
        j = i
    return inside


def _polygon_area_m2(polygon: list) -> float:
    """Approximate polygon area in m² using shoelace on flat-earth coords."""
    if len(polygon) < 3:
        return 0.0
    lat0, lon0 = polygon[0]
    cos0 = math.cos(math.radians(lat0))
    pts = [
        ((lat - lat0) * 111_320.0, (lon - lon0) * 111_320.0 * cos0)
        for lat, lon in polygon
    ]
    n = len(pts)
    area = 0.0
    for i in range(n):
        j = (i + 1) % n
        area += pts[i][0] * pts[j][1]
        area -= pts[j][0] * pts[i][1]
    return abs(area) / 2.0


class GeofenceNode(Node):

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

        self.declare_parameter('fence_vertices',  [])        # flat list: lat0,lon0,lat1,lon1,...
        self.declare_parameter('check_rate_hz',   2.0)
        self.declare_parameter('stop_on_exit',    True)      # publish zero /cmd_vel
        self.declare_parameter('map_frame',       'map')

        self._polygon: list[tuple[float, float]] = []
        self._fix:     Optional[NavSatFix]        = None
        self._violated = False

        self._load_fence()

        # ── Subscriptions ────────────────────────────────────────────────────
        self.create_subscription(NavSatFix, '/gps/fix',           self._fix_cb,  10)
        self.create_subscription(PoseArray, '/outdoor/waypoints', self._wps_cb,  10)

        # ── Publishers ───────────────────────────────────────────────────────
        self._abort_pub  = self.create_publisher(Bool,   '/outdoor/abort',        10)
        self._status_pub = self.create_publisher(String, '/outdoor/fence_status', 10)
        self._vel_pub    = self.create_publisher(Twist,  '/cmd_vel',              10)

        # ── Service ──────────────────────────────────────────────────────────
        self.create_service(Trigger, '/outdoor/reload_fence', self._reload_cb)

        rate = self.get_parameter('check_rate_hz').value
        self.create_timer(1.0 / rate, self._check)

        area = _polygon_area_m2(self._polygon) if self._polygon else 0.0
        self.get_logger().info(
            f'geofence: {len(self._polygon)}-vertex polygon, area≈{area:.0f} m²'
        )

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _fix_cb(self, msg: NavSatFix) -> None:
        self._fix = msg

    def _wps_cb(self, msg: PoseArray) -> None:
        """Pre-check: warn if any map-frame waypoint is suspiciously far out."""
        # In V1 we can't easily reverse-project map→GPS here without TF/EKF,
        # so we just log the waypoint count and rely on the robot fix check.
        self.get_logger().info(
            f'geofence: {len(msg.poses)} waypoints received (pre-flight GPS check pending)'
        )

    def _reload_cb(self, _req, response):
        self._load_fence()
        response.success = True
        response.message = f'Fence reloaded: {len(self._polygon)} vertices'
        return response

    # ── Fence check ───────────────────────────────────────────────────────────

    def _check(self) -> None:
        if not self._polygon:
            self._pub_status('no_fence_configured')
            return
        if self._fix is None:
            self._pub_status('no_gps')
            return
        if self._fix.status.status < 0:
            return   # no fix — don't trigger on bad data

        lat, lon = self._fix.latitude, self._fix.longitude
        inside = _point_in_polygon(lat, lon, self._polygon)

        if inside:
            self._violated = False
            self._pub_status('inside')
        else:
            if not self._violated:
                self.get_logger().warn(
                    f'GEOFENCE VIOLATION: robot at ({lat:.6f}, {lon:.6f}) is outside fence'
                )
                self._violated = True

            self._pub_status(f'outside:{lat:.6f},{lon:.6f}')

            # Publish abort
            abort = Bool()
            abort.data = True
            self._abort_pub.publish(abort)

            # Emergency stop
            if self.get_parameter('stop_on_exit').value:
                self._vel_pub.publish(Twist())   # zero twist = stop

    # ── Helpers ───────────────────────────────────────────────────────────────

    def _load_fence(self) -> None:
        """Parse fence_vertices parameter: flat [lat0, lon0, lat1, lon1, ...]."""
        raw = self.get_parameter('fence_vertices').value
        if not raw or len(raw) < 6:
            self._polygon = []
            self.get_logger().warn('geofence: no valid fence_vertices — geofence inactive')
            return
        pairs = list(raw)
        self._polygon = [(pairs[i], pairs[i + 1]) for i in range(0, len(pairs) - 1, 2)]
        self.get_logger().info(f'geofence: loaded {len(self._polygon)}-vertex polygon')

    def _pub_status(self, s: str) -> None:
        msg = String()
        msg.data = s
        self._status_pub.publish(msg)


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


if __name__ == '__main__':
    main()
