"""
terrain_adaptation_node.py — Surface detection and dynamic adaptation (Issue #142).

Overview
────────
  Analyses IMU vibration to classify the terrain underfoot, detects incline
  and wheel slip, then publishes advisory outputs that other nodes use to
  adapt speed limits, PID aggressiveness, and motor bias.

Pipeline (50 Hz)
────────────────
  1. IMU acc  → TerrainAnalyzer → roughness, surface_type, grip
  2. IMU pitch (quaternion) → InclineDetector → incline_rad
  3. cmd_vel + odom  → SlipDetector → slip_ratio
  4. (roughness, incline, grip, slip) → TerrainAdapter → speed_scale,
     pid_scales, motor_bias
  5. Publish Terrain msg + advisory scalars + history log

Subscribes
──────────
  /saltybot/imu                sensor_msgs/Imu       — vibration + pitch
  <odom_topic>                 nav_msgs/Odometry     — actual speed for slip
  /cmd_vel                     geometry_msgs/Twist   — commanded speed for slip

Publishes
─────────
  /saltybot/terrain            saltybot_terrain_msgs/Terrain — full terrain state
  /saltybot/terrain_speed_scale std_msgs/Float32             — speed multiplier
  /saltybot/terrain_pid_scales  std_msgs/String (JSON)       — kp/ki/kd scales
  /saltybot/terrain_motor_bias  std_msgs/Float32             — incline compensation
  /saltybot/terrain_history     std_msgs/String (JSON)       — change log

Parameters
──────────
  control_rate            50.0    Hz
  odom_topic              /saltybot/rover_odom
  vibration_window_s      0.5     s (RMS window)
  roughness_max_ms2       3.0     m/s² (normalisation ceiling)
  hysteresis_count        5       samples before class change
  incline_lpf_tau_s       1.5     s (incline low-pass time constant)
  max_incline_rad         0.52    rad (≈30°)
  slip_threshold          0.30    (slip flag threshold)
  slip_min_speed_ms       0.15    m/s (ignore slip below this speed)
  slip_smoothing_alpha    0.20
  k_bias                  1.0     (incline motor bias gain)
  min_speed_scale         0.15    (speed_scale floor)
  history_max_entries     200
  imu_timeout_s           0.5     s (zero outputs if IMU goes silent)
"""

import json
import math
import time

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

from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from std_msgs.msg import Float32, String

from saltybot_terrain_adaptation.terrain_analyzer import TerrainAnalyzer
from saltybot_terrain_adaptation.slip_detector import SlipDetector
from saltybot_terrain_adaptation.incline_detector import InclineDetector
from saltybot_terrain_adaptation.terrain_adapter import (
    TerrainAdapter,
    TerrainHistory,
    TerrainState,
)

try:
    from saltybot_terrain_msgs.msg import Terrain as TerrainMsg
    _TERRAIN_MSG_OK = True
except ImportError:
    _TERRAIN_MSG_OK = False


def _quaternion_to_pitch(qx: float, qy: float, qz: float, qw: float) -> float:
    sinp = 2.0 * (qw * qy - qz * qx)
    return math.asin(max(-1.0, min(1.0, sinp)))


class TerrainAdaptationNode(Node):

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

        # ── Parameters ───────────────────────────────────────────────────────
        self._declare_params()
        p = self._load_params()

        # ── Sub-systems ──────────────────────────────────────────────────────
        self._analyzer = TerrainAnalyzer(
            max_variance_ms2=p["roughness_max_ms2"],
            window_s=p["vibration_window_s"],
            control_rate_hz=p["control_rate"],
            hysteresis_count=p["hysteresis_count"],
        )
        self._incline = InclineDetector(
            lpf_tau_s=p["incline_lpf_tau_s"],
            control_rate_hz=p["control_rate"],
            max_incline_rad=p["max_incline_rad"],
        )
        self._slip = SlipDetector(
            slip_threshold=p["slip_threshold"],
            min_speed_ms=p["slip_min_speed_ms"],
            smoothing_alpha=p["slip_smoothing_alpha"],
        )
        self._adapter = TerrainAdapter(
            max_incline_rad=p["max_incline_rad"],
            min_speed_scale=p["min_speed_scale"],
        )
        self._history = TerrainHistory(max_entries=p["history_max_entries"])

        # ── State ────────────────────────────────────────────────────────────
        self._latest_imu: Imu | None = None
        self._actual_speed_ms: float = 0.0
        self._cmd_speed_ms: float = 0.0
        self._last_imu_t: float = 0.0
        self._last_ctrl_t: float = time.monotonic()

        # ── QoS ──────────────────────────────────────────────────────────────
        reliable = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=10,
        )
        best_effort = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        # ── Subscriptions ────────────────────────────────────────────────────
        self.create_subscription(Imu,     "/saltybot/imu",         self._imu_cb,    best_effort)
        self.create_subscription(Odometry, p["odom_topic"],         self._odom_cb,   reliable)
        self.create_subscription(Twist,   "/cmd_vel",              self._cmd_vel_cb, reliable)

        # ── Publishers ───────────────────────────────────────────────────────
        self._terrain_pub      = self.create_publisher(String,  "/saltybot/terrain",              reliable)
        self._speed_scale_pub  = self.create_publisher(Float32, "/saltybot/terrain_speed_scale",  reliable)
        self._pid_scales_pub   = self.create_publisher(String,  "/saltybot/terrain_pid_scales",   reliable)
        self._motor_bias_pub   = self.create_publisher(Float32, "/saltybot/terrain_motor_bias",   reliable)
        self._history_pub      = self.create_publisher(String,  "/saltybot/terrain_history",      reliable)

        # Try to create a typed Terrain publisher if message is available
        self._typed_terrain_pub = None
        if _TERRAIN_MSG_OK:
            self._typed_terrain_pub = self.create_publisher(
                TerrainMsg, "/saltybot/terrain_typed", reliable
            )

        # ── Timer ────────────────────────────────────────────────────────────
        rate = p["control_rate"]
        self._timer = self.create_timer(1.0 / rate, self._control_cb)

        self.get_logger().info(
            f"TerrainAdaptationNode ready  rate={rate}Hz  "
            f"odom={p['odom_topic']}"
        )

    # ── Parameter management ─────────────────────────────────────────────────

    def _declare_params(self) -> None:
        self.declare_parameter("control_rate",         50.0)
        self.declare_parameter("odom_topic",           "/saltybot/rover_odom")
        self.declare_parameter("vibration_window_s",    0.5)
        self.declare_parameter("roughness_max_ms2",     3.0)
        self.declare_parameter("hysteresis_count",      5)
        self.declare_parameter("incline_lpf_tau_s",     1.5)
        self.declare_parameter("max_incline_rad",       0.52)
        self.declare_parameter("slip_threshold",        0.30)
        self.declare_parameter("slip_min_speed_ms",     0.15)
        self.declare_parameter("slip_smoothing_alpha",  0.20)
        self.declare_parameter("k_bias",                1.0)
        self.declare_parameter("min_speed_scale",       0.15)
        self.declare_parameter("history_max_entries",   200)
        self.declare_parameter("imu_timeout_s",         0.5)

    def _load_params(self) -> dict:
        g = self.get_parameter
        return {k: g(k).value for k in [
            "control_rate", "odom_topic",
            "vibration_window_s", "roughness_max_ms2", "hysteresis_count",
            "incline_lpf_tau_s", "max_incline_rad",
            "slip_threshold", "slip_min_speed_ms", "slip_smoothing_alpha",
            "k_bias", "min_speed_scale", "history_max_entries", "imu_timeout_s",
        ]}

    # ── Subscriptions ─────────────────────────────────────────────────────────

    def _imu_cb(self, msg: Imu) -> None:
        self._latest_imu = msg
        self._last_imu_t = time.monotonic()

    def _odom_cb(self, msg: Odometry) -> None:
        self._actual_speed_ms = msg.twist.twist.linear.x

    def _cmd_vel_cb(self, msg: Twist) -> None:
        self._cmd_speed_ms = msg.linear.x

    # ── 50 Hz control loop ────────────────────────────────────────────────────

    def _control_cb(self) -> None:
        p = self._load_params()
        now = time.monotonic()
        dt = now - self._last_ctrl_t
        self._last_ctrl_t = now

        # IMU watchdog
        imu_age = (now - self._last_imu_t) if self._last_imu_t > 0.0 else 1e9
        if imu_age > p["imu_timeout_s"] or self._latest_imu is None:
            self._publish_safe_defaults()
            return

        imu = self._latest_imu
        ax = imu.linear_acceleration.x
        ay = imu.linear_acceleration.y
        az = imu.linear_acceleration.z
        q  = imu.orientation

        # 1. Vibration analysis
        analysis = self._analyzer.update(ax, ay, az)
        roughness = analysis.roughness    if analysis else 0.0
        surf_type = analysis.surface_type if analysis else "unknown"
        grip      = analysis.grip         if analysis else 0.6

        # 2. Incline detection
        pitch = _quaternion_to_pitch(q.x, q.y, q.z, q.w)
        incline = self._incline.update(pitch, dt)

        # 3. Slip detection
        slip_ratio, is_slipping = self._slip.update(
            self._cmd_speed_ms, self._actual_speed_ms
        )

        # 4. Build terrain state
        raw_state = TerrainState(
            surface_type=surf_type,
            roughness=roughness,
            incline_rad=incline,
            grip=grip,
            slip_ratio=slip_ratio,
            timestamp_s=now,
        )
        state = self._adapter.apply(raw_state)

        # 5. Terrain history + logging
        type_changed = self._history.add(state)
        if type_changed:
            self.get_logger().info(
                f"Terrain: {surf_type}  rough={roughness:.2f}  "
                f"grip={grip:.2f}  incline={math.degrees(incline):.1f}°  "
                f"speed_scale={state.speed_scale:.2f}"
            )
        if is_slipping:
            self.get_logger().warn(
                f"Wheel slip detected: ratio={slip_ratio:.2f}  "
                f"cmd={self._cmd_speed_ms:.2f}m/s  actual={self._actual_speed_ms:.2f}m/s"
            )

        # 6. Publish
        self._publish_terrain_json(state)
        self._publish_speed_scale(state.speed_scale)
        self._publish_pid_scales(self._adapter.pid_scales(state))
        self._publish_motor_bias(
            self._adapter.motor_bias(state, k_bias=p["k_bias"])
        )
        # Publish history on every type change (not every tick)
        if type_changed:
            self._publish_history()

        # Typed message (if saltybot_terrain_msgs is built)
        if self._typed_terrain_pub is not None and _TERRAIN_MSG_OK:
            self._publish_typed(state)

    # ── Publishers ────────────────────────────────────────────────────────────

    def _publish_safe_defaults(self) -> None:
        self._publish_speed_scale(1.0)
        self._publish_pid_scales({"kp": 1.0, "ki": 1.0, "kd": 1.0})
        self._publish_motor_bias(0.0)

    def _publish_terrain_json(self, state: TerrainState) -> None:
        msg = String()
        msg.data = json.dumps({
            "surface_type": state.surface_type,
            "roughness":    round(state.roughness, 3),
            "incline_rad":  round(state.incline_rad, 4),
            "incline_deg":  round(math.degrees(state.incline_rad), 2),
            "grip":         round(state.grip, 3),
            "slip_ratio":   round(state.slip_ratio, 3),
            "speed_scale":  round(state.speed_scale, 3),
        })
        self._terrain_pub.publish(msg)

    def _publish_speed_scale(self, scale: float) -> None:
        msg = Float32()
        msg.data = float(scale)
        self._speed_scale_pub.publish(msg)

    def _publish_pid_scales(self, scales: dict) -> None:
        msg = String()
        msg.data = json.dumps({k: round(v, 4) for k, v in scales.items()})
        self._pid_scales_pub.publish(msg)

    def _publish_motor_bias(self, bias: float) -> None:
        msg = Float32()
        msg.data = float(bias)
        self._motor_bias_pub.publish(msg)

    def _publish_history(self) -> None:
        msg = String()
        msg.data = json.dumps(self._history.to_json_log())
        self._history_pub.publish(msg)

    def _publish_typed(self, state: TerrainState) -> None:
        msg = TerrainMsg()
        msg.stamp           = self.get_clock().now().to_msg()
        msg.surface_type    = state.surface_type
        msg.roughness       = float(state.roughness)
        msg.incline_rad     = float(state.incline_rad)
        msg.grip            = float(state.grip)
        msg.slip_ratio      = float(state.slip_ratio)
        msg.speed_scale     = float(state.speed_scale)
        self._typed_terrain_pub.publish(msg)


# ── Entry point ───────────────────────────────────────────────────────────────

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


if __name__ == "__main__":
    main()
