#!/usr/bin/env python3
"""
Patrol mode for SaltyBot. Autonomous waypoint circuit with:
- Idle behaviors at each waypoint (pan-tilt sweep, greetings)
- Person detection → pause patrol → social mode → resume
- Battery monitoring (dock when <25%)
- Teleop override
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import Pose, PoseStamped, Quaternion
from std_msgs.msg import String, Float32
from sensor_msgs.msg import Image
from vision_msgs.msg import Detection2DArray
import yaml
import random
import time
from enum import Enum
from threading import Thread


class PatrolState(Enum):
    IDLE = 0
    NAVIGATE = 1
    IDLE_BEHAVIOR = 2
    PAUSE_PERSON = 3
    SOCIAL_MODE = 4
    RETURN_TO_DOCK = 5


class PatrolNode(Node):
    def __init__(self):
        super().__init__('patrol_node')

        # Parameters
        self.declare_parameter('patrol_params_file', 'patrol_params.yaml')
        self.declare_parameter('min_idle_time', 10)  # seconds
        self.declare_parameter('max_idle_time', 30)  # seconds
        self.declare_parameter('battery_dock_threshold', 25.0)  # percent
        self.declare_parameter('person_pause_timeout', 30.0)  # seconds

        self.params_file = self.get_parameter('patrol_params_file').value
        self.min_idle = self.get_parameter('min_idle_time').value
        self.max_idle = self.get_parameter('max_idle_time').value
        self.battery_dock = self.get_parameter('battery_dock_threshold').value
        self.person_timeout = self.get_parameter('person_pause_timeout').value

        # State
        self.state = PatrolState.IDLE
        self.waypoints = []
        self.current_wp_idx = 0
        self.battery_level = 100.0
        self.is_person_present = False
        self.teleop_override = False
        self.patrol_enabled = False

        # Load waypoints
        self.load_waypoints()

        # Publishers
        self.state_pub = self.create_publisher(String, '/saltybot/patrol_state', 10)
        self.greet_pub = self.create_publisher(String, '/saltybot/speech_text', 10)
        self.pan_tilt_pub = self.create_publisher(String, '/saltybot/pan_tilt_cmd', 10)

        # Subscribers
        self.create_subscription(Float32, '/saltybot/battery_percent', self.battery_callback, 10)
        self.create_subscription(Detection2DArray, '/saltybot/person_detections', self.person_callback, 10)
        self.create_subscription(String, '/saltybot/teleop_cmd', self.teleop_callback, 10)
        self.create_subscription(String, '/saltybot/voice_cmd', self.voice_callback, 10)

        # Nav2 action client
        self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

        # Timer for patrol loop
        self.patrol_timer = self.create_timer(0.5, self.patrol_loop)

        self.get_logger().info("Patrol node initialized")

    def load_waypoints(self):
        """Load patrol waypoints from YAML file."""
        try:
            with open(self.params_file, 'r') as f:
                params = yaml.safe_load(f)
                self.waypoints = params.get('patrol_waypoints', [])
                self.get_logger().info(f"Loaded {len(self.waypoints)} waypoints")

                if self.waypoints:
                    # Randomize order
                    random.shuffle(self.waypoints)
        except FileNotFoundError:
            self.get_logger().warn(f"Waypoints file not found: {self.params_file}")
            self.waypoints = []

    def battery_callback(self, msg):
        """Update battery level."""
        self.battery_level = msg.data
        if self.battery_level < self.battery_dock:
            self.get_logger().warn(f"Low battery: {self.battery_level}%")
            if self.state != PatrolState.RETURN_TO_DOCK:
                self.state = PatrolState.RETURN_TO_DOCK

    def person_callback(self, msg):
        """Detect if person is present."""
        self.is_person_present = len(msg.detections) > 0
        if self.is_person_present and self.state == PatrolState.NAVIGATE:
            self.get_logger().info("Person detected - pausing patrol")
            self.state = PatrolState.PAUSE_PERSON

    def teleop_callback(self, msg):
        """Handle teleop override."""
        if msg.data == "take_control":
            self.teleop_override = True
            self.patrol_enabled = False
            self.state = PatrolState.IDLE
            self.get_logger().info("Teleop override active")
        elif msg.data == "release_control":
            self.teleop_override = False
            self.get_logger().info("Teleop override released")

    def voice_callback(self, msg):
        """Handle voice commands."""
        if "patrol" in msg.data.lower():
            if "start" in msg.data.lower():
                self.patrol_enabled = True
                self.state = PatrolState.NAVIGATE
                self.get_logger().info("Patrol started by voice")
            elif "stop" in msg.data.lower():
                self.patrol_enabled = False
                self.state = PatrolState.IDLE
                self.get_logger().info("Patrol stopped by voice")

    def publish_state(self):
        """Publish current patrol state."""
        msg = String()
        msg.data = f"state:{self.state.name},battery:{self.battery_level:.1f}%,waypoint:{self.current_wp_idx}/{len(self.waypoints)}"
        self.state_pub.publish(msg)

    def navigate_to_waypoint(self):
        """Send navigation goal to Nav2."""
        if not self.waypoints or self.current_wp_idx >= len(self.waypoints):
            self.current_wp_idx = 0

        wp = self.waypoints[self.current_wp_idx]

        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = wp['x']
        goal.pose.pose.position.y = wp['y']
        goal.pose.pose.position.z = 0.0

        # Quaternion from yaw (z rotation)
        import math
        yaw = wp.get('yaw', 0.0)
        goal.pose.pose.orientation.w = math.cos(yaw / 2)
        goal.pose.pose.orientation.z = math.sin(yaw / 2)

        self.nav_client.wait_for_server()
        self.nav_client.send_goal_async(goal)
        self.state = PatrolState.NAVIGATE
        self.get_logger().info(f"Navigating to waypoint {self.current_wp_idx}")

    def idle_behavior(self):
        """Perform idle behaviors at waypoint."""
        # Pan-tilt sweep
        cmd = String()
        cmd.data = "sweep"
        self.pan_tilt_pub.publish(cmd)

        # Random greeting
        greetings = [
            "Hello! How are you today?",
            "Hi there! Great to see you!",
            "Welcome! Nice to meet you!",
            "Hey there! What's up?",
        ]
        greet = String()
        greet.data = random.choice(greetings)
        self.greet_pub.publish(greet)

        # Idle for random duration
        idle_time = random.uniform(self.min_idle, self.max_idle)
        self.get_logger().info(f"Idle behavior: {idle_time:.1f}s")
        time.sleep(idle_time)

    def return_to_dock(self):
        """Navigate back to dock (home)."""
        self.get_logger().info("Returning to dock")
        # Send to dock location (0, 0)
        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = 0.0
        goal.pose.pose.position.y = 0.0
        goal.pose.pose.position.z = 0.0
        goal.pose.pose.orientation.w = 1.0

        self.nav_client.wait_for_server()
        self.nav_client.send_goal_async(goal)
        self.state = PatrolState.RETURN_TO_DOCK

    def patrol_loop(self):
        """Main patrol state machine."""
        self.publish_state()

        if not self.patrol_enabled or self.teleop_override:
            return

        if self.state == PatrolState.IDLE:
            if self.patrol_enabled and not self.teleop_override:
                self.navigate_to_waypoint()

        elif self.state == PatrolState.NAVIGATE:
            # Wait for navigation to complete
            pass

        elif self.state == PatrolState.IDLE_BEHAVIOR:
            # Perform idle behaviors at waypoint
            self.idle_behavior()
            self.current_wp_idx = (self.current_wp_idx + 1) % len(self.waypoints)
            self.navigate_to_waypoint()

        elif self.state == PatrolState.PAUSE_PERSON:
            # Pause and enter social mode
            if self.is_person_present:
                # Wait for person to leave
                pass
            else:
                self.state = PatrolState.NAVIGATE
                self.navigate_to_waypoint()

        elif self.state == PatrolState.RETURN_TO_DOCK:
            # Docking - wait for battery to charge
            if self.battery_level > 80.0:
                self.patrol_enabled = False
                self.state = PatrolState.IDLE


def main(args=None):
    rclpy.init(args=args)
    node = PatrolNode()

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
