#!/usr/bin/env python3
"""360° LIDAR obstacle avoidance node for SaltyBot.

Uses RPLIDAR A1M8 for 360° scanning with speed-dependent safety zones.
Publishes emergency alerts and filtered cmd_vel with obstacle avoidance.

Subscribed topics:
  /scan (sensor_msgs/LaserScan) - RPLIDAR A1M8 scan data
  /cmd_vel (geometry_msgs/Twist) - Input velocity command

Published topics:
  /saltybot/obstacle_alert (std_msgs/Bool) - Obstacle detected alert
  /cmd_vel_safe (geometry_msgs/Twist) - Filtered velocity (avoidance applied)
  /saltybot/lidar_avoidance_status (std_msgs/String) - Debug status JSON
"""

import json
import math
from typing import Tuple

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from std_msgs.msg import Bool, String


class LidarAvoidanceNode(Node):
    """360° LIDAR obstacle avoidance with speed-dependent safety zones."""

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

        # Safety parameters
        self.declare_parameter("emergency_stop_distance", 0.5)  # m
        self.declare_parameter("max_speed_reference", 5.56)  # m/s (20 km/h)
        self.declare_parameter("safety_zone_at_max_speed", 3.0)  # m
        self.declare_parameter("min_safety_zone", 0.6)  # m (below emergency stop)
        self.declare_parameter("angle_window_degrees", 60)  # ±30° forward cone
        self.declare_parameter("debounce_frames", 2)

        self.emergency_stop_distance = self.get_parameter("emergency_stop_distance").value
        self.max_speed_reference = self.get_parameter("max_speed_reference").value
        self.safety_zone_at_max_speed = self.get_parameter("safety_zone_at_max_speed").value
        self.min_safety_zone = self.get_parameter("min_safety_zone").value
        self.angle_window_degrees = self.get_parameter("angle_window_degrees").value
        self.debounce_frames = self.get_parameter("debounce_frames").value

        # State tracking
        self.obstacle_detected = False
        self.consecutive_obstacles = 0
        self.current_speed = 0.0
        self.last_scan_ranges = None
        self.emergency_stop_triggered = False

        # Subscriptions
        self.create_subscription(LaserScan, "/scan", self._on_scan, 10)
        self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)

        # Publishers
        self.pub_alert = self.create_publisher(Bool, "/saltybot/obstacle_alert", 10)
        self.pub_safe_vel = self.create_publisher(Twist, "/cmd_vel_safe", 10)
        self.pub_status = self.create_publisher(
            String, "/saltybot/lidar_avoidance_status", 10
        )

        self.get_logger().info(
            f"LIDAR avoidance initialized:\n"
            f"  Emergency stop: {self.emergency_stop_distance}m\n"
            f"  Speed-dependent zone: {self.safety_zone_at_max_speed}m @ {self.max_speed_reference}m/s\n"
            f"  Forward angle window: ±{self.angle_window_degrees / 2}°\n"
            f"  Min safety zone: {self.min_safety_zone}m"
        )

    def _on_scan(self, msg: LaserScan) -> None:
        """Process LIDAR scan data and check for obstacles."""
        self.last_scan_ranges = msg.ranges

        # Calculate safety threshold based on current speed
        safety_distance = self._get_safety_distance(self.current_speed)

        # Get minimum distance in forward cone
        min_distance, angle_deg = self._get_min_distance_forward(msg)

        # Check for obstacles
        obstacle_now = min_distance < safety_distance
        emergency_stop_now = min_distance < self.emergency_stop_distance

        # Debounce obstacle detection
        if obstacle_now:
            self.consecutive_obstacles += 1
        else:
            self.consecutive_obstacles = 0

        obstacle_detected_debounced = (
            self.consecutive_obstacles >= self.debounce_frames
        )

        # Handle state changes
        if emergency_stop_now and not self.emergency_stop_triggered:
            self.get_logger().error(
                f"EMERGENCY STOP! Obstacle at {min_distance:.2f}m, {angle_deg:.1f}°"
            )
            self.emergency_stop_triggered = True
        elif not emergency_stop_now:
            self.emergency_stop_triggered = False

        if obstacle_detected_debounced != self.obstacle_detected:
            self.obstacle_detected = obstacle_detected_debounced
            if self.obstacle_detected:
                self.get_logger().warn(
                    f"Obstacle detected: {min_distance:.2f}m @ {angle_deg:.1f}°"
                )
            else:
                self.get_logger().info("Obstacle cleared")

        # Publish alert
        alert_msg = Bool(data=self.obstacle_detected)
        self.pub_alert.publish(alert_msg)

        # Publish status
        status = {
            "min_distance": round(min_distance, 3),
            "angle_deg": round(angle_deg, 1),
            "safety_distance": round(safety_distance, 3),
            "obstacle_detected": self.obstacle_detected,
            "emergency_stop": self.emergency_stop_triggered,
            "current_speed": round(self.current_speed, 3),
        }
        status_msg = String(data=json.dumps(status))
        self.pub_status.publish(status_msg)

    def _on_cmd_vel(self, msg: Twist) -> None:
        """Process incoming velocity command and apply obstacle avoidance."""
        self.current_speed = math.sqrt(msg.linear.x**2 + msg.linear.y**2)

        # Apply safety filtering
        if self.emergency_stop_triggered:
            # Emergency stop: zero out all motion
            safe_vel = Twist()
        elif self.obstacle_detected:
            # Obstacle in path: reduce speed
            safe_vel = Twist()
            safety_distance = self._get_safety_distance(self.current_speed)
            min_distance, _ = self._get_min_distance_forward(self.last_scan_ranges)

            if self.last_scan_ranges is not None and min_distance > 0:
                # Linear interpolation of allowed speed based on distance to obstacle
                if min_distance < safety_distance:
                    # Scale velocity from 0 to current based on distance
                    scale_factor = (min_distance - self.emergency_stop_distance) / (
                        safety_distance - self.emergency_stop_distance
                    )
                    scale_factor = max(0.0, min(1.0, scale_factor))

                    safe_vel.linear.x = msg.linear.x * scale_factor
                    safe_vel.linear.y = msg.linear.y * scale_factor
                    safe_vel.angular.z = msg.angular.z * scale_factor
                else:
                    safe_vel = msg
            else:
                safe_vel = msg
        else:
            # No obstacle: pass through command
            safe_vel = msg

        self.pub_safe_vel.publish(safe_vel)

    def _get_safety_distance(self, speed: float) -> float:
        """Calculate speed-dependent safety zone distance.

        Linear interpolation: 0 m/s → min_safety_zone, max_speed → safety_zone_at_max_speed
        """
        if speed <= 0:
            return self.min_safety_zone

        if speed >= self.max_speed_reference:
            return self.safety_zone_at_max_speed

        # Linear interpolation
        ratio = speed / self.max_speed_reference
        safety = self.min_safety_zone + ratio * (
            self.safety_zone_at_max_speed - self.min_safety_zone
        )
        return safety

    def _get_min_distance_forward(self, scan_data) -> Tuple[float, float]:
        """Get minimum distance in forward cone."""
        if isinstance(scan_data, LaserScan):
            ranges = scan_data.ranges
            angle_min = scan_data.angle_min
            angle_increment = scan_data.angle_increment
        else:
            # scan_data is a tuple of (ranges, angle_min, angle_increment) or list
            if not scan_data:
                return float('inf'), 0.0
            ranges = scan_data
            angle_min = -math.pi  # Assume standard LIDAR orientation
            angle_increment = 2 * math.pi / len(ranges)

        half_window = self.angle_window_degrees / 2.0 * math.pi / 180.0
        min_distance = float('inf')
        min_angle = 0.0

        for i, distance in enumerate(ranges):
            if distance <= 0 or math.isnan(distance) or math.isinf(distance):
                continue

            angle_rad = angle_min + i * angle_increment

            # Normalize to -π to π
            while angle_rad > math.pi:
                angle_rad -= 2 * math.pi
            while angle_rad < -math.pi:
                angle_rad += 2 * math.pi

            # Check forward window
            if abs(angle_rad) <= half_window:
                if distance < min_distance:
                    min_distance = distance
                    min_angle = angle_rad

        return min_distance, math.degrees(min_angle)


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


if __name__ == "__main__":
    main()
