"""
velocity_ramp_node.py — Smooth velocity ramp controller (Issue #350).

Subscribes to raw /cmd_vel commands and republishes them on /cmd_vel_smooth
after applying independent acceleration and deceleration limits to the linear-x
and angular-z components.

An emergency stop (both linear and angular targets exactly 0.0) bypasses the
ramp and forces the output to zero immediately.

Subscribes
----------
  /cmd_vel          geometry_msgs/Twist   raw velocity commands

Publishes
---------
  /cmd_vel_smooth   geometry_msgs/Twist   rate-limited velocity commands

Parameters
----------
  max_lin_accel   float   0.5    Maximum linear  acceleration (m/s²)
  max_lin_decel   float   0.5    Maximum linear  deceleration (m/s²)
  max_ang_accel   float   1.0    Maximum angular acceleration (rad/s²)
  max_ang_decel   float   1.0    Maximum angular deceleration (rad/s²)
  rate_hz         float   50.0   Control loop rate (Hz)
"""

from __future__ import annotations

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

from geometry_msgs.msg import Twist

from ._velocity_ramp import VelocityRamp


_SUB_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)
_PUB_QOS = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)


class VelocityRampNode(Node):

    def __init__(self) -> None:
        super().__init__('velocity_ramp_node')

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter('max_lin_accel', 0.5)
        self.declare_parameter('max_lin_decel', 0.5)
        self.declare_parameter('max_ang_accel', 1.0)
        self.declare_parameter('max_ang_decel', 1.0)
        self.declare_parameter('rate_hz',       50.0)

        p = self.get_parameter
        rate_hz      = max(float(p('rate_hz').value),      1.0)
        max_lin_acc  = max(float(p('max_lin_accel').value), 1e-3)
        max_lin_dec  = max(float(p('max_lin_decel').value), 1e-3)
        max_ang_acc  = max(float(p('max_ang_accel').value), 1e-3)
        max_ang_dec  = max(float(p('max_ang_decel').value), 1e-3)

        dt = 1.0 / rate_hz

        # ── Ramp state ───────────────────────────────────────────────────────
        self._ramp = VelocityRamp(
            dt            = dt,
            max_lin_accel = max_lin_acc,
            max_lin_decel = max_lin_dec,
            max_ang_accel = max_ang_acc,
            max_ang_decel = max_ang_dec,
        )
        self._target_lin: float = 0.0
        self._target_ang: float = 0.0

        # ── Subscriber ───────────────────────────────────────────────────────
        self._sub = self.create_subscription(
            Twist, '/cmd_vel', self._on_cmd_vel, _SUB_QOS,
        )

        # ── Publisher + timer ────────────────────────────────────────────────
        self._pub   = self.create_publisher(Twist, '/cmd_vel_smooth', _PUB_QOS)
        self._timer = self.create_timer(dt, self._step)

        self.get_logger().info(
            f'velocity_ramp_node ready — '
            f'rate={rate_hz:.0f}Hz  '
            f'lin_accel={max_lin_acc}m/s²  lin_decel={max_lin_dec}m/s²  '
            f'ang_accel={max_ang_acc}rad/s²  ang_decel={max_ang_dec}rad/s²'
        )

    # ── Callbacks ─────────────────────────────────────────────────────────────

    def _on_cmd_vel(self, msg: Twist) -> None:
        self._target_lin = float(msg.linear.x)
        self._target_ang = float(msg.angular.z)

    def _step(self) -> None:
        lin, ang = self._ramp.step(self._target_lin, self._target_ang)

        out = Twist()
        out.linear.x  = lin
        out.angular.z = ang
        self._pub.publish(out)


def main(args=None) -> None:
    rclpy.init(args=args)
    node = VelocityRampNode()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()
