#!/usr/bin/env python3

from collections import deque

import rclpy
from autoware_adapi_v1_msgs.msg import OperationModeState
from rclpy.node import Node
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy


class OperationModeStateFallback(Node):
    def __init__(self):
        super().__init__("planning_debug_operation_mode_fallback")

        self.topic = (
            self.declare_parameter("topic", "/system/operation_mode/state")
            .get_parameter_value()
            .string_value
        )
        self.period_sec = (
            self.declare_parameter("period_sec", 0.5).get_parameter_value().double_value
        )
        self.timeout_sec = (
            self.declare_parameter("timeout_sec", 1.5).get_parameter_value().double_value
        )

        qos = QoSProfile(depth=1)
        qos.reliability = ReliabilityPolicy.RELIABLE
        qos.durability = DurabilityPolicy.TRANSIENT_LOCAL

        self.pub = self.create_publisher(OperationModeState, self.topic, qos)

        sub_qos = QoSProfile(depth=1)
        sub_qos.reliability = ReliabilityPolicy.RELIABLE
        sub_qos.durability = DurabilityPolicy.VOLATILE
        self.sub = self.create_subscription(
            OperationModeState, self.topic, self.on_state, sub_qos
        )

        self.timer = self.create_timer(self.period_sec, self.on_timer)
        self.publishing_fallback = False
        self.last_msg_time = None
        self.own_stamps = deque(maxlen=32)

        self.get_logger().info(
            f"Monitoring {self.topic}; publishing STOP fallback when no fresh message exists."
        )

    @staticmethod
    def stamp_key(stamp):
        return stamp.sec, stamp.nanosec

    def on_state(self, msg):
        if self.stamp_key(msg.stamp) in self.own_stamps:
            return
        self.last_msg_time = self.get_clock().now()

    def has_external_publisher(self):
        own_name = self.get_name()
        own_namespace = self.get_namespace()

        for info in self.get_publishers_info_by_topic(self.topic):
            if info.node_name != own_name or info.node_namespace != own_namespace:
                return True
        return False

    def has_fresh_message(self):
        if self.last_msg_time is None:
            return False

        age_sec = (self.get_clock().now() - self.last_msg_time).nanoseconds * 1e-9
        return age_sec <= self.timeout_sec

    def on_timer(self):
        if self.has_fresh_message():
            if self.publishing_fallback:
                self.get_logger().info(
                    f"Detected fresh {self.topic}; fallback is idle."
                )
                self.publishing_fallback = False
            return

        msg = OperationModeState()
        msg.stamp = self.get_clock().now().to_msg()
        msg.mode = OperationModeState.STOP
        msg.is_autoware_control_enabled = False
        msg.is_in_transition = False
        msg.is_stop_mode_available = True
        msg.is_autonomous_mode_available = False
        msg.is_local_mode_available = False
        msg.is_remote_mode_available = False
        self.own_stamps.append(self.stamp_key(msg.stamp))
        self.pub.publish(msg)

        if not self.publishing_fallback:
            reason = (
                "no external publisher"
                if not self.has_external_publisher()
                else f"no fresh message within {self.timeout_sec:.1f}s"
            )
            self.get_logger().warn(
                f"{reason} on {self.topic}; publishing STOP fallback for planning."
            )
            self.publishing_fallback = True


def main():
    rclpy.init()
    node = OperationModeStateFallback()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()