#!/usr/bin/env python3
"""
terrain_speed_adapter_node.py — Issue #469: Auto-adapt speed based on terrain.

Subscribes to cmd_vel and terrain_state, applies terrain-based speed adaptation,
and publishes adjusted velocity commands.

Integration point: cmd_vel → [speed adaptation] → cmd_vel_terrain
(Existing velocity_ramp_node still handles acceleration/deceleration ramping)
"""

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

from geometry_msgs.msg import Twist
from saltybot_social_msgs.msg import TerrainState


class TerrainSpeedAdapterNode(Node):
    """Adapt cmd_vel speed based on terrain state."""

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

        # Parameters
        self.declare_parameter('enable_terrain_adaptation', True)
        self.declare_parameter('min_speed_ratio', 0.1)
        self.declare_parameter('debug_logging', False)

        self.enable_adaptation = self.get_parameter('enable_terrain_adaptation').value
        self.min_speed_ratio = self.get_parameter('min_speed_ratio').value
        self.debug_logging = self.get_parameter('debug_logging').value

        # QoS profile
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            depth=1
        )

        # Subscribers
        self.cmd_vel_sub = self.create_subscription(
            Twist,
            '/cmd_vel',
            self._cmd_vel_callback,
            qos_profile=qos
        )

        self.terrain_sub = self.create_subscription(
            TerrainState,
            '/saltybot/terrain_state',
            self._terrain_callback,
            qos_profile=qos
        )

        # Publisher
        self.cmd_vel_adapted_pub = self.create_publisher(
            Twist,
            '/cmd_vel_terrain',
            10
        )

        # State
        self.latest_cmd_vel = Twist()
        self.latest_terrain = None
        self.speed_ratio = 1.0

        self.get_logger().info(
            f'Terrain speed adapter initialized (enabled={self.enable_adaptation}, '
            f'min_ratio={self.min_speed_ratio})'
        )

    def _cmd_vel_callback(self, msg: Twist):
        """Store latest cmd_vel and publish adapted version."""
        self.latest_cmd_vel = msg

        # Adapt speed and publish
        self._publish_adapted_velocity()

    def _terrain_callback(self, msg: TerrainState):
        """Update terrain state and speed ratio."""
        self.latest_terrain = msg
        self.speed_ratio = max(
            self.min_speed_ratio,
            msg.recommended_speed_ratio
        )

        if self.debug_logging:
            terrain_names = {
                0: 'UNKNOWN',
                1: 'PAVEMENT',
                2: 'GRASS',
                3: 'GRAVEL',
                4: 'SAND',
                5: 'INDOOR',
            }
            self.get_logger().info(
                f'Terrain updated: {terrain_names.get(msg.terrain_type, "?")} '
                f'(roughness={msg.roughness:.2f}, speed_ratio={self.speed_ratio:.2f})'
            )

    def _publish_adapted_velocity(self):
        """Apply terrain adaptation and publish."""
        if not self.enable_adaptation:
            # Pass through if disabled
            self.cmd_vel_adapted_pub.publish(self.latest_cmd_vel)
            return

        # Create adapted velocity
        adapted = Twist()

        # Apply speed ratio only to linear velocity (preserve steering)
        adapted.linear.x = self.latest_cmd_vel.linear.x * self.speed_ratio
        adapted.linear.y = self.latest_cmd_vel.linear.y * self.speed_ratio
        adapted.linear.z = self.latest_cmd_vel.linear.z * self.speed_ratio

        # Angular velocity unchanged (terrain doesn't affect steering capability)
        adapted.angular = self.latest_cmd_vel.angular

        self.cmd_vel_adapted_pub.publish(adapted)

        if self.debug_logging and self.latest_terrain is not None:
            self.get_logger().debug(
                f'Velocity adapted: {self.latest_cmd_vel.linear.x:.3f} → '
                f'{adapted.linear.x:.3f} (ratio={self.speed_ratio:.2f})'
            )


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


if __name__ == '__main__':
    main()
