#!/usr/bin/env python3
"""Smooth acceleration limiter for velocity commands.

Applies configurable acceleration and deceleration rate limits to prevent jerky motion.

Subscribed topics:
  /cmd_vel (geometry_msgs/Twist) - Target velocity command

Published topics:
  /cmd_vel_smooth (geometry_msgs/Twist) - Rate-limited velocity command

Prevents sudden velocity changes by limiting rate of change to:
  - linear: 0.5 m/s² (configurable)
  - angular: 1.0 rad/s² (configurable)
"""

from typing import Optional

import rclpy
from rclpy.node import Node
from rclpy.timer import Timer
from geometry_msgs.msg import Twist


class AccelLimiterNode(Node):
    """ROS2 node for smooth acceleration limiting."""

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

        # Parameters
        self.declare_parameter("frequency", 50)  # Hz
        frequency = self.get_parameter("frequency").value
        self.declare_parameter("linear_accel_limit", 0.5)  # m/s²
        self.declare_parameter("angular_accel_limit", 1.0)  # rad/s²

        self.linear_accel_limit = self.get_parameter("linear_accel_limit").value
        self.angular_accel_limit = self.get_parameter("angular_accel_limit").value

        self.period = 1.0 / frequency

        # Latest target velocity from subscription
        self.target_vel: Optional[Twist] = None

        # Current smoothed velocity (published)
        self.current_vel = Twist()

        # Subscription to target velocity
        self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10)

        # Publisher for smoothed velocity
        self.pub_smooth = self.create_publisher(Twist, "/cmd_vel_smooth", 10)

        # Timer for rate limiting at configured frequency
        self.timer: Timer = self.create_timer(self.period, self._timer_callback)

        self.get_logger().info(
            f"Acceleration limiter initialized at {frequency}Hz. "
            f"Linear accel limit: {self.linear_accel_limit} m/s². "
            f"Angular accel limit: {self.angular_accel_limit} rad/s²"
        )

    def _on_cmd_vel(self, msg: Twist) -> None:
        """Update target velocity from cmd_vel subscription."""
        self.target_vel = msg

    def _timer_callback(self) -> None:
        """Apply rate limiting and publish smoothed velocity."""
        if self.target_vel is None:
            # No target yet, publish zero velocity
            self.pub_smooth.publish(self.current_vel)
            return

        # Create new velocity with rate-limited changes
        limited_vel = Twist()

        # Rate limit linear components
        limited_vel.linear.x = self._limit_component(
            self.current_vel.linear.x,
            self.target_vel.linear.x,
            self.linear_accel_limit,
        )
        limited_vel.linear.y = self._limit_component(
            self.current_vel.linear.y,
            self.target_vel.linear.y,
            self.linear_accel_limit,
        )
        limited_vel.linear.z = self._limit_component(
            self.current_vel.linear.z,
            self.target_vel.linear.z,
            self.linear_accel_limit,
        )

        # Rate limit angular components
        limited_vel.angular.x = self._limit_component(
            self.current_vel.angular.x,
            self.target_vel.angular.x,
            self.angular_accel_limit,
        )
        limited_vel.angular.y = self._limit_component(
            self.current_vel.angular.y,
            self.target_vel.angular.y,
            self.angular_accel_limit,
        )
        limited_vel.angular.z = self._limit_component(
            self.current_vel.angular.z,
            self.target_vel.angular.z,
            self.angular_accel_limit,
        )

        # Update current velocity and publish
        self.current_vel = limited_vel
        self.pub_smooth.publish(self.current_vel)

    def _limit_component(
        self, current: float, target: float, accel_limit: float
    ) -> float:
        """Apply acceleration limit to a single velocity component.

        Args:
            current: Current velocity value (m/s or rad/s)
            target: Target velocity value (m/s or rad/s)
            accel_limit: Maximum acceleration (m/s² or rad/s²)

        Returns:
            Rate-limited velocity value
        """
        # Compute desired change
        change = target - current

        # Maximum change allowed in this time period: accel_limit * dt
        max_change = accel_limit * self.period

        # Clamp change to limits
        if change > max_change:
            return current + max_change
        elif change < -max_change:
            return current - max_change
        else:
            return target


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


if __name__ == "__main__":
    main()
