#!/usr/bin/env python3
"""Geofence safety system for SaltyBot.

Subscribes to /phone/gps and /odom. Enforces circle/polygon geofence.
Three zones: SAFE, WARNING (2m buffer), VIOLATION.
WARNING: slow + concern face + TTS + amber LED.
VIOLATION: stop + auto-return + red LED.
"""

import math
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from std_msgs.msg import String, UInt8

class GeofenceState:
    SAFE = 0
    WARNING = 1
    VIOLATION = 2

class GeofenceNode(Node):
    def __init__(self):
        super().__init__("geofence")
        self.declare_parameter("type", "circle")
        self.declare_parameter("circle.center_lat", 0.0)
        self.declare_parameter("circle.center_lon", 0.0)
        self.declare_parameter("circle.radius", 50.0)
        self.declare_parameter("polygon", [])
        self.declare_parameter("warning_distance", 2.0)
        self.declare_parameter("auto_return.enabled", True)
        self.declare_parameter("auto_return.speed", 0.5)
        self.declare_parameter("tts.enabled", True)
        self.declare_parameter("tts.warning_message", "Approaching boundary")
        self.declare_parameter("tts.violation_message", "Boundary violation, returning home")

        self.geofence_type = self.get_parameter("type").value
        self.warning_distance = self.get_parameter("warning_distance").value
        self.auto_return_enabled = self.get_parameter("auto_return.enabled").value
        self.auto_return_speed = self.get_parameter("auto_return.speed").value
        self.tts_enabled = self.get_parameter("tts.enabled").value

        if self.geofence_type == "circle":
            self.circle_center_lat = self.get_parameter("circle.center_lat").value
            self.circle_center_lon = self.get_parameter("circle.center_lon").value
            self.circle_radius = self.get_parameter("circle.radius").value

        self.geofence_state_pub = self.create_publisher(UInt8, "/saltybot/geofence_state", 1)
        self.led_pub = self.create_publisher(String, "/saltybot/led_command", 1)
        self.tts_pub = self.create_publisher(String, "/saltybot/tts_command", 1)
        self.emotion_pub = self.create_publisher(String, "/saltybot/emotion_command", 1)
        self.cmd_vel_pub = self.create_publisher(Twist, "/cmd_vel", 1)

        self.create_subscription(NavSatFix, "/phone/gps", self._gps_callback, 1)
        self.create_subscription(Odometry, "/odom", self._odom_callback, 1)

        self.current_state = GeofenceState.SAFE
        self.last_state = GeofenceState.SAFE
        self.current_lat = 0.0
        self.current_lon = 0.0
        self.returning_home = False
        self.home_lat = None
        self.home_lon = None
        self.get_logger().info(f"Geofence initialized: type={self.geofence_type}")

    def _gps_callback(self, msg: NavSatFix):
        if msg.latitude == 0 and msg.longitude == 0:
            return
        self.current_lat = msg.latitude
        self.current_lon = msg.longitude
        if self.home_lat is None:
            self.home_lat = msg.latitude
            self.home_lon = msg.longitude
            self.get_logger().info(f"Home: ({self.home_lat:.6f}, {self.home_lon:.6f})")
        self._update_geofence_state()

    def _odom_callback(self, msg: Odometry):
        pass

    def _update_geofence_state(self):
        if self.geofence_type == "circle":
            distance = self._haversine_distance(
                self.current_lat, self.current_lon,
                self.circle_center_lat, self.circle_center_lon
            )
            if distance <= self.circle_radius - self.warning_distance:
                new_state = GeofenceState.SAFE
            elif distance <= self.circle_radius:
                new_state = GeofenceState.WARNING
            else:
                new_state = GeofenceState.VIOLATION
        else:
            inside = self._point_in_polygon(self.current_lat, self.current_lon, self.polygon)
            distance = self._min_distance_to_polygon(self.current_lat, self.current_lon, self.polygon)
            if inside and distance > self.warning_distance:
                new_state = GeofenceState.SAFE
            elif inside or distance <= self.warning_distance:
                new_state = GeofenceState.WARNING
            else:
                new_state = GeofenceState.VIOLATION

        if new_state != self.current_state:
            self.current_state = new_state
            self._on_state_change()

    def _on_state_change(self):
        self.geofence_state_pub.publish(UInt8(data=self.current_state))
        if self.current_state == GeofenceState.SAFE:
            self.led_pub.publish(String(data="off"))
            self.returning_home = False
        elif self.current_state == GeofenceState.WARNING:
            self.led_pub.publish(String(data="amber"))
            self.emotion_pub.publish(String(data="concerned"))
            if self.tts_enabled:
                self.tts_pub.publish(String(data=self.get_parameter("tts.warning_message").value))
        elif self.current_state == GeofenceState.VIOLATION:
            self.led_pub.publish(String(data="red"))
            self.emotion_pub.publish(String(data="concerned"))
            if self.tts_enabled:
                self.tts_pub.publish(String(data=self.get_parameter("tts.violation_message").value))
            twist = Twist()
            self.cmd_vel_pub.publish(twist)
            if self.auto_return_enabled and not self.returning_home and self.home_lat:
                self.returning_home = True
                self._initiate_auto_return()
        self.last_state = self.current_state

    def _initiate_auto_return(self):
        self.get_logger().warn("Auto-return initiated")
        twist = Twist()
        twist.linear.x = self.auto_return_speed
        self.cmd_vel_pub.publish(twist)

    @staticmethod
    def _haversine_distance(lat1, lon1, lat2, lon2):
        R = 6371000
        phi1 = math.radians(lat1)
        phi2 = math.radians(lat2)
        delta_phi = math.radians(lat2 - lat1)
        delta_lambda = math.radians(lon2 - lon1)
        a = math.sin(delta_phi / 2) ** 2 + math.cos(phi1) * math.cos(phi2) * math.sin(delta_lambda / 2) ** 2
        c = 2 * math.asin(math.sqrt(a))
        return R * c

    @staticmethod
    def _point_in_polygon(lat, lon, polygon):
        if not polygon or len(polygon) < 3:
            return False
        n = len(polygon)
        inside = False
        p1_lat, p1_lon = polygon[0]
        for i in range(1, n + 1):
            p2_lat, p2_lon = polygon[i % n]
            if lat > min(p1_lat, p2_lat):
                if lat <= max(p1_lat, p2_lat):
                    if lon <= max(p1_lon, p2_lon):
                        if p1_lat != p2_lat:
                            xinters = (lat - p1_lat) * (p2_lon - p1_lon) / (p2_lat - p1_lat) + p1_lon
                        if p1_lat == p2_lat or lon <= xinters:
                            inside = not inside
            p1_lat, p1_lon = p2_lat, p2_lon
        return inside

    @staticmethod
    def _min_distance_to_polygon(lat, lon, polygon):
        if not polygon or len(polygon) < 2:
            return float("inf")
        min_dist = float("inf")
        for i in range(len(polygon)):
            p1 = polygon[i]
            p2 = polygon[(i + 1) % len(polygon)]
            d = abs((p2[0] - p1[0]) * (p1[1] - lon) - (p1[0] - lat) * (p2[1] - p1[1]))
            d /= max(math.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2), 1e-6)
            min_dist = min(min_dist, d)
        return min_dist

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()
