#!/usr/bin/env python3
"""
Curiosity behavior for SaltyBot — autonomous exploration when idle.

Activates when idle >60s with no people detected. Performs frontier exploration:
- Turns toward sounds (audio_direction)
- Approaches colorful/moving objects (object detection)
- Takes panoramic photos at interesting spots
- Self-narrates findings via TTS
- Respects geofence and dynamic obstacles
- Auto-stops after 10min

Publishes:
  /saltybot/curiosity_state  String  State machine updates
"""

import rclpy
from rclpy.node import Node
from rclpy.action import ActionClient
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from nav2_msgs.action import NavigateToPose
from geometry_msgs.msg import Pose, PoseStamped, Quaternion, Twist
from std_msgs.msg import String, Float32, Bool
from sensor_msgs.msg import Image
from vision_msgs.msg import Detection2DArray

import math
import time
import random
from enum import Enum
from collections import deque


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
)


class CuriosityState(Enum):
    IDLE = 0
    WAITING_FOR_IDLE = 1
    EXPLORING = 2
    INVESTIGATING = 3
    RETURNING = 4


class CuriosityNode(Node):
    def __init__(self):
        super().__init__('curiosity_node')

        # Parameters
        self.declare_parameter('curiosity_level', 0.7)
        self.declare_parameter('idle_threshold_sec', 60.0)
        self.declare_parameter('exploration_max_duration_sec', 600.0)  # 10min
        self.declare_parameter('exploration_radius_m', 5.0)
        self.declare_parameter('pan_tilt_step_deg', 15.0)
        self.declare_parameter('min_sound_activity', 0.1)

        self.curiosity_level = self.get_parameter('curiosity_level').value
        self.idle_threshold = self.get_parameter('idle_threshold_sec').value
        self.max_exploration_duration = self.get_parameter('exploration_max_duration_sec').value
        self.exploration_radius = self.get_parameter('exploration_radius_m').value
        self.pan_tilt_step = self.get_parameter('pan_tilt_step_deg').value
        self.min_sound_activity = self.get_parameter('min_sound_activity').value

        # State tracking
        self.state = CuriosityState.IDLE
        self.last_person_time = time.time()
        self.last_activity_time = time.time()
        self.exploration_start_time = None
        self.current_location = Pose()
        self.home_location = Pose()

        # Sensors
        self.audio_bearing = 0.0
        self.audio_activity = False
        self.recent_detections = deque(maxlen=5)
        self.battery_level = 100.0
        self.geofence_limit = None
        self.obstacles = []

        # Narration queue
        self.narration_queue = []

        # Publishers
        self.state_pub = self.create_publisher(
            String, '/saltybot/curiosity_state', 10
        )
        self.narration_pub = self.create_publisher(
            String, '/saltybot/tts_request', 10
        )
        self.pan_tilt_pub = self.create_publisher(
            String, '/saltybot/pan_tilt_cmd', 10
        )
        self.velocity_pub = self.create_publisher(
            Twist, '/cmd_vel', 10
        )

        # Subscribers
        self.create_subscription(
            Float32, '/saltybot/audio_direction', self.audio_bearing_callback, _SENSOR_QOS
        )
        self.create_subscription(
            Bool, '/saltybot/audio_activity', self.audio_activity_callback, _SENSOR_QOS
        )
        self.create_subscription(
            Detection2DArray, '/saltybot/person_detections', self.person_callback, _SENSOR_QOS
        )
        self.create_subscription(
            Detection2DArray, '/saltybot/object_detections', self.object_callback, _SENSOR_QOS
        )
        self.create_subscription(
            Float32, '/saltybot/battery_percent', self.battery_callback, 10
        )
        self.create_subscription(
            String, '/saltybot/geofence_status', self.geofence_callback, 10
        )

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

        # Main loop timer
        self.timer = self.create_timer(1.0, self.curiosity_loop)

        self.get_logger().info(
            f"Curiosity node initialized (level={self.curiosity_level}, "
            f"idle_threshold={self.idle_threshold}s, max_duration={self.max_exploration_duration}s)"
        )

    def audio_bearing_callback(self, msg):
        """Update detected audio bearing (0-360 degrees)."""
        self.audio_bearing = msg.data
        self.last_activity_time = time.time()

    def audio_activity_callback(self, msg):
        """Track if audio activity detected."""
        self.audio_activity = msg.data
        if msg.data:
            self.last_activity_time = time.time()

    def person_callback(self, msg):
        """Update last person detection time."""
        if len(msg.detections) > 0:
            self.last_person_time = time.time()

    def object_callback(self, msg):
        """Track interesting objects (colorful/moving)."""
        for detection in msg.detections:
            # Store recent detections with confidence
            if hasattr(detection, 'results') and len(detection.results) > 0:
                obj = {
                    'class': detection.results[0].class_name if hasattr(detection.results[0], 'class_name') else 'unknown',
                    'confidence': detection.results[0].score if hasattr(detection.results[0], 'score') else 0.0,
                }
                self.recent_detections.append(obj)
                self.last_activity_time = time.time()

    def battery_callback(self, msg):
        """Track battery level."""
        self.battery_level = msg.data

    def geofence_callback(self, msg):
        """Track geofence status."""
        data = msg.data
        if "limit:" in data:
            try:
                limit_str = data.split("limit:")[1].strip()
                self.geofence_limit = float(limit_str)
            except (ValueError, IndexError):
                pass

    def publish_state(self, details=""):
        """Publish current curiosity state."""
        msg = String()
        state_str = f"state:{self.state.name}"
        if details:
            state_str += f",{details}"
        msg.data = state_str
        self.state_pub.publish(msg)

    def narrate(self, text):
        """Queue text for TTS narration."""
        msg = String()
        msg.data = text
        self.narration_pub.publish(msg)
        self.get_logger().info(f"Narrating: {text}")

    def pan_tilt_toward(self, bearing_deg):
        """Command pan-tilt to look toward bearing."""
        msg = String()
        msg.data = f"pan:{int(bearing_deg % 360)}"
        self.pan_tilt_pub.publish(msg)

    def turn_toward_sound(self):
        """Turn to face detected sound source."""
        if self.audio_activity and self.min_sound_activity > 0:
            self.get_logger().info(f"Sound detected at bearing {self.audio_bearing}°")
            self.narrate(f"Interesting! I hear something at {int(self.audio_bearing)} degrees!")
            self.pan_tilt_toward(self.audio_bearing)
            return True
        return False

    def approach_interesting_object(self):
        """Identify and approach interesting objects if available."""
        if not self.recent_detections:
            return False

        # Look for colorful or moving objects
        interesting_classes = ['ball', 'toy', 'person', 'car', 'dog', 'cat', 'bird']
        for obj in self.recent_detections:
            class_name = obj.get('class', '').lower()
            confidence = obj.get('confidence', 0.0)

            if any(keyword in class_name for keyword in interesting_classes) and confidence > 0.6:
                self.narrate(f"Ooh, a {obj['class']}! Let me check it out.")
                # Would navigate toward object here (simplified)
                return True

        return False

    def is_idle(self):
        """Check if we've been idle long enough to activate curiosity."""
        time_since_person = time.time() - self.last_person_time
        time_since_activity = time.time() - self.last_activity_time

        # Idle if no person nearby AND no recent activity
        return (
            time_since_person > self.idle_threshold
            and time_since_activity > (self.idle_threshold / 2)
        )

    def should_explore(self):
        """Decide whether to start exploration based on curiosity level."""
        # Higher curiosity = more likely to explore when idle
        return random.random() < self.curiosity_level

    def explore_frontier(self):
        """Generate frontier exploration goal."""
        # Pick random direction within exploration radius
        distance = random.uniform(1.0, self.exploration_radius)
        angle = random.uniform(0, 2 * math.pi)

        goal_x = self.current_location.position.x + distance * math.cos(angle)
        goal_y = self.current_location.position.y + distance * math.sin(angle)

        # Check against geofence
        if self.geofence_limit:
            dist_from_home = math.sqrt(goal_x**2 + goal_y**2)
            if dist_from_home > self.geofence_limit:
                # Clamp to geofence boundary
                scale = (self.geofence_limit * 0.9) / dist_from_home
                goal_x *= scale
                goal_y *= scale

        return goal_x, goal_y

    def navigate_to_exploration_point(self):
        """Send navigation goal for exploration."""
        goal_x, goal_y = self.explore_frontier()

        goal = NavigateToPose.Goal()
        goal.pose.header.frame_id = "map"
        goal.pose.header.stamp = self.get_clock().now().to_msg()
        goal.pose.pose.position.x = goal_x
        goal.pose.pose.position.y = goal_y
        goal.pose.pose.position.z = 0.0
        goal.pose.pose.orientation.w = 1.0

        try:
            self.nav_client.wait_for_server(timeout_sec=2.0)
            self.nav_client.send_goal_async(goal)
            self.get_logger().info(f"Exploring toward ({goal_x:.2f}, {goal_y:.2f})")
        except Exception as e:
            self.get_logger().warn(f"Navigation unavailable: {e}")

    def return_home(self):
        """Navigate back to starting location."""
        self.narrate("I've seen enough. Heading back home.")
        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

        try:
            self.nav_client.wait_for_server(timeout_sec=2.0)
            self.nav_client.send_goal_async(goal)
        except Exception as e:
            self.get_logger().warn(f"Navigation unavailable: {e}")

    def curiosity_loop(self):
        """Main curiosity state machine."""
        self.publish_state()

        if self.state == CuriosityState.IDLE:
            if self.is_idle() and self.should_explore() and self.battery_level > 30:
                self.state = CuriosityState.EXPLORING
                self.exploration_start_time = time.time()
                self.narrate("Hmm, nothing to do. Let me explore!")
                self.get_logger().info("Curiosity activated!")

        elif self.state == CuriosityState.EXPLORING:
            # Check timeout
            duration = time.time() - self.exploration_start_time
            if duration > self.max_exploration_duration:
                self.state = CuriosityState.RETURNING
                return

            # React to stimuli
            if self.turn_toward_sound():
                pass  # Already turned toward sound
            elif self.approach_interesting_object():
                pass  # Already approaching object
            else:
                # Random frontier exploration
                if random.random() < 0.3:  # 30% chance to explore frontier each cycle
                    self.navigate_to_exploration_point()

            # Periodic narration
            if random.random() < 0.1:
                observations = [
                    "Interesting things over here!",
                    "I wonder what's in that direction.",
                    "The world is so big!",
                    "What could be around the corner?",
                ]
                self.narrate(random.choice(observations))

        elif self.state == CuriosityState.RETURNING:
            self.return_home()
            self.state = CuriosityState.IDLE
            self.exploration_start_time = None


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

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


if __name__ == '__main__':
    main()
