"""
vo_drift_node.py — Visual odometry drift detector (Issue #260).

Compares the cumulative path lengths of visual odometry and wheel odometry
over a sliding window.  When the absolute difference exceeds the configured
threshold the node flags drift, allowing the system to warn operators,
inflate VO covariance, or fall back to wheel-only localisation.

Subscribes (BEST_EFFORT):
  /camera/odom            nav_msgs/Odometry   visual odometry
  /odom                   nav_msgs/Odometry   wheel odometry

  → For this robot remap to /saltybot/visual_odom + /saltybot/rover_odom.

Publishes:
  /saltybot/vo_drift_detected   std_msgs/Bool     True while drifting
  /saltybot/vo_drift_magnitude  std_msgs/Float32  drift magnitude (metres)

Parameters
----------
vo_topic           str    /camera/odom       Visual odometry source topic
wheel_topic        str    /odom              Wheel odometry source topic
drift_threshold_m  float  0.5                Drift flag threshold (metres)
window_s           float  10.0               Comparison window (seconds)
publish_hz         float  2.0                Output publication rate (Hz)
max_buffer_age_s   float  30.0               Max age of stored samples (s)
"""

from __future__ import annotations

import time

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

from nav_msgs.msg import Odometry
from std_msgs.msg import Bool, Float32

from ._vo_drift import OdomBuffer, OdomSample, compute_drift


_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=4,
)


class VoDriftNode(Node):

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

        self.declare_parameter('vo_topic',          '/camera/odom')
        self.declare_parameter('wheel_topic',       '/odom')
        self.declare_parameter('drift_threshold_m', 0.5)
        self.declare_parameter('window_s',          10.0)
        self.declare_parameter('publish_hz',        2.0)
        self.declare_parameter('max_buffer_age_s',  30.0)

        vo_topic       = self.get_parameter('vo_topic').value
        wheel_topic    = self.get_parameter('wheel_topic').value
        self._thresh   = self.get_parameter('drift_threshold_m').value
        self._window_s = self.get_parameter('window_s').value
        publish_hz     = self.get_parameter('publish_hz').value
        max_age        = self.get_parameter('max_buffer_age_s').value

        self._vo_buf    = OdomBuffer(max_age_s=max_age)
        self._wheel_buf = OdomBuffer(max_age_s=max_age)

        self.create_subscription(
            Odometry, vo_topic, self._on_vo, _SENSOR_QOS)
        self.create_subscription(
            Odometry, wheel_topic, self._on_wheel, _SENSOR_QOS)

        self._pub_detected  = self.create_publisher(
            Bool,    '/saltybot/vo_drift_detected',   10)
        self._pub_magnitude = self.create_publisher(
            Float32, '/saltybot/vo_drift_magnitude',  10)

        self.create_timer(1.0 / publish_hz, self._tick)

        self.get_logger().info(
            f'vo_drift_node ready — '
            f'vo={vo_topic} wheel={wheel_topic} '
            f'threshold={self._thresh}m window={self._window_s}s'
        )

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

    def _on_vo(self, msg: Odometry) -> None:
        s = _odom_to_sample(msg)
        self._vo_buf.push(s)

    def _on_wheel(self, msg: Odometry) -> None:
        s = _odom_to_sample(msg)
        self._wheel_buf.push(s)

    # ── Publish tick ──────────────────────────────────────────────────────────

    def _tick(self) -> None:
        now    = time.monotonic()
        result = compute_drift(
            self._vo_buf, self._wheel_buf,
            window_s=self._window_s,
            drift_threshold_m=self._thresh,
            now=now,
        )

        if result.is_drifting:
            self.get_logger().warn(
                f'VO drift detected: {result.drift_m:.3f}m '
                f'(vo={result.vo_path_m:.3f}m wheel={result.wheel_path_m:.3f}m '
                f'over {result.window_s:.1f}s)',
                throttle_duration_sec=5.0,
            )

        det_msg = Bool()
        det_msg.data = result.is_drifting
        self._pub_detected.publish(det_msg)

        mag_msg = Float32()
        mag_msg.data = float(result.drift_m)
        self._pub_magnitude.publish(mag_msg)


# ── Helpers ───────────────────────────────────────────────────────────────────

def _odom_to_sample(msg: Odometry) -> OdomSample:
    """Convert nav_msgs/Odometry to OdomSample using monotonic clock."""
    return OdomSample(
        t=time.monotonic(),
        x=msg.pose.pose.position.x,
        y=msg.pose.pose.position.y,
    )


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


if __name__ == '__main__':
    main()
