#!/usr/bin/env python3
"""Convert Twist yaw-rate commands to Ackermann steering commands."""

import math
import time

import rclpy
from ackermann_msgs.msg import AckermannDriveStamped
from geometry_msgs.msg import Twist
from rclpy.node import Node


class AckermannCmdBridge(Node):
    def __init__(self):
        super().__init__("planner_ackermann_cmd_bridge")
        self.declare_parameter("input_topic", "/cmd_vel")
        self.declare_parameter("output_topic", "/ackermann_cmd")
        self.declare_parameter("wheelbase", 0.143)
        self.declare_parameter("max_steering_angle", 0.60)
        self.declare_parameter("frame_id", "base_link")
        self.declare_parameter("command_timeout", 0.25)
        self.declare_parameter("watchdog_rate", 10.0)
        self.declare_parameter("state_log_period", 1.0)

        input_topic = str(self.get_parameter("input_topic").value)
        output_topic = str(self.get_parameter("output_topic").value)
        self.wheelbase = float(self.get_parameter("wheelbase").value)
        self.max_steering_angle = abs(
            float(self.get_parameter("max_steering_angle").value)
        )
        self.frame_id = str(self.get_parameter("frame_id").value)
        self.command_timeout = max(
            0.05, float(self.get_parameter("command_timeout").value)
        )
        self.state_log_period = max(
            0.0, float(self.get_parameter("state_log_period").value)
        )
        if self.wheelbase <= 0.0:
            raise ValueError("wheelbase must be positive")
        if self.max_steering_angle <= 0.0:
            raise ValueError("max_steering_angle must be positive")

        self.publisher = self.create_publisher(
            AckermannDriveStamped, output_topic, 10
        )
        self.subscription = self.create_subscription(
            Twist, input_topic, self._cmd_callback, 10
        )
        self.last_command_time = None
        self.last_idle_publish_time = 0.0
        self.last_state_log_time = 0.0
        self.last_input_linear = 0.0
        self.last_input_angular = 0.0
        self.last_output_velocity = 0.0
        self.last_output_steering = 0.0
        self.last_steering_saturated = False
        watchdog_rate = max(2.0, float(self.get_parameter("watchdog_rate").value))
        self.watchdog_timer = self.create_timer(1.0 / watchdog_rate, self._watchdog)
        self.get_logger().info(
            f"bridge {input_topic} -> {output_topic}, "
            f"wheelbase={self.wheelbase:.3f} m, "
            f"steering_limit={self.max_steering_angle:.3f} rad, "
            f"command_timeout={self.command_timeout:.3f} s"
        )

    def _cmd_callback(self, command):
        self.last_command_time = time.monotonic()
        velocity = command.linear.x
        self.last_input_linear = velocity
        self.last_input_angular = command.angular.z
        if abs(velocity) <= 1e-6 or abs(command.angular.z) <= 1e-6:
            steering = 0.0
            raw_steering = 0.0
        else:
            raw_steering = math.atan(self.wheelbase * command.angular.z / velocity)
            steering = raw_steering
        steering = max(
            -self.max_steering_angle,
            min(self.max_steering_angle, steering),
        )
        self.last_steering_saturated = abs(raw_steering - steering) > 1e-6

        self._publish(velocity, steering)

    def _watchdog(self):
        now = time.monotonic()
        command_stale = (
            self.last_command_time is None
            or now - self.last_command_time > self.command_timeout
        )
        if command_stale and now - self.last_idle_publish_time >= 0.5:
            self._publish(0.0, 0.0)
            self.last_idle_publish_time = now

    def _publish(self, velocity, steering):
        self.last_output_velocity = float(velocity)
        self.last_output_steering = float(steering)
        output = AckermannDriveStamped()
        output.header.stamp = self.get_clock().now().to_msg()
        output.header.frame_id = self.frame_id
        output.drive.speed = self.last_output_velocity
        output.drive.steering_angle = self.last_output_steering
        self.publisher.publish(output)
        self._maybe_log_state()

    def _maybe_log_state(self):
        if self.state_log_period <= 0.0:
            return
        now = time.monotonic()
        if now - self.last_state_log_time < self.state_log_period:
            return
        self.last_state_log_time = now
        if self.last_command_time is None:
            command_age = None
            stale = True
        else:
            command_age = max(0.0, now - self.last_command_time)
            stale = command_age > self.command_timeout
        age_text = "none" if command_age is None else f"{command_age:.3f}"
        self.get_logger().info(
            "ackermann_state "
            f"input_twist=({self.last_input_linear:.3f},{self.last_input_angular:.3f}) "
            f"output=(speed={self.last_output_velocity:.3f},"
            f"steering={self.last_output_steering:.3f}) "
            f"command_age={age_text} stale={stale} "
            f"steering_saturated={self.last_steering_saturated}"
        )

    def stop(self):
        for _ in range(3):
            self._publish(0.0, 0.0)


def main():
    rclpy.init()
    node = AckermannCmdBridge()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.stop()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
