"""
emergency_node.py — Emergency behavior system orchestration (Issue #169).

Overview
────────
  Aggregates threats from four independent detectors and drives the
  EmergencyFSM.  Overrides /cmd_vel when an emergency is active.  Escalates
  via /saltybot/e_stop and /saltybot/critical_alert for CRITICAL events.

Pipeline (20 Hz)
────────────────
  1. LaserScan callback → ObstacleDetector → ThreatEvent
  2. IMU callback       → FallDetector + BumpDetector → ThreatEvent (×2)
  3. Odom callback      → StuckDetector (fed in timer) → ThreatEvent
  4. 20 Hz timer        → highest_threat() → EmergencyFSM.tick()
                        → publish overriding cmd_vel or pass-through
                        → publish /saltybot/emergency + /saltybot/recovery_action

Subscribes
──────────
  /scan                    sensor_msgs/LaserScan  — obstacle detection
  /saltybot/imu            sensor_msgs/Imu        — fall + bump detection
  <odom_topic>             nav_msgs/Odometry      — stuck + speed tracking
  /cmd_vel                 geometry_msgs/Twist    — nav commands (pass-through)

Publishes
─────────
  /saltybot/cmd_vel_out    geometry_msgs/Twist    — muxed cmd_vel (to drive nodes)
  /saltybot/e_stop         std_msgs/Bool          — emergency stop flag
  /saltybot/alert_beep     std_msgs/Empty         — beep trigger (MINOR)
  /saltybot/alert_flash    std_msgs/Empty         — LED flash trigger (MAJOR)
  /saltybot/critical_alert std_msgs/String (JSON) — CRITICAL event for MQTT bridge
  /saltybot/emergency      saltybot_emergency_msgs/EmergencyEvent
  /saltybot/recovery_action saltybot_emergency_msgs/RecoveryAction

Parameters
──────────
  control_rate             20.0    Hz
  odom_topic               /saltybot/rover_odom
  forward_scan_angle_rad   0.785   rad  (±45° forward sector for obstacle check)
  stop_distance_m          0.30    m
  critical_distance_m      0.10    m
  min_cmd_speed_ms         0.05    m/s
  minor_tilt_rad           0.20    rad
  major_tilt_rad           0.35    rad
  critical_tilt_rad        0.52    rad
  floor_drop_m             0.15    m
  stuck_timeout_s          3.0     s
  jerk_threshold_ms3       8.0     m/s³
  critical_jerk_threshold_ms3  25.0 m/s³
  stopped_ms               0.03    m/s
  major_count_threshold    3
  escalation_window_s      10.0    s
  suppression_s            1.0     s
  reverse_speed_ms        -0.15    m/s
  reverse_distance_m       0.30    m
  angular_speed_rads       0.60    rad/s
  turn_angle_rad           1.5708  rad
  retry_timeout_s          3.0     s
  clear_hold_s             0.50    s
  max_retries              3
"""

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, LaserScan
from std_msgs.msg import Bool, Empty, String

from saltybot_emergency.alert_manager import AlertLevel
from saltybot_emergency.emergency_fsm import EmergencyFSM, EmergencyInputs, EmergencyState
from saltybot_emergency.threat_detector import (
    BumpDetector,
    FallDetector,
    ObstacleDetector,
    StuckDetector,
    ThreatEvent,
    ThreatType,
    highest_threat,
)

try:
    from saltybot_emergency_msgs.msg import EmergencyEvent, RecoveryAction
    _MSGS_OK = True
except ImportError:
    _MSGS_OK = False


def _quaternion_to_pitch_roll(qx, qy, qz, qw):
    pitch = math.asin(max(-1.0, min(1.0, 2.0 * (qw * qy - qz * qx))))
    roll  = math.atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy))
    return pitch, roll


class EmergencyNode(Node):

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

        self._declare_params()
        p = self._load_params()

        # ── Detectors ────────────────────────────────────────────────────────
        self._obstacle = ObstacleDetector(
            stop_distance_m=p["stop_distance_m"],
            critical_distance_m=p["critical_distance_m"],
            min_speed_ms=p["min_cmd_speed_ms"],
        )
        self._fall = FallDetector(
            minor_tilt_rad=p["minor_tilt_rad"],
            major_tilt_rad=p["major_tilt_rad"],
            critical_tilt_rad=p["critical_tilt_rad"],
            floor_drop_m=p["floor_drop_m"],
        )
        self._stuck = StuckDetector(
            stuck_timeout_s=p["stuck_timeout_s"],
            min_cmd_ms=p["min_cmd_speed_ms"],
        )
        self._bump = BumpDetector(
            jerk_threshold_ms3=p["jerk_threshold_ms3"],
            critical_jerk_threshold_ms3=p["critical_jerk_threshold_ms3"],
        )
        self._fsm = EmergencyFSM(
            stopped_ms=p["stopped_ms"],
            major_count_threshold=p["major_count_threshold"],
            escalation_window_s=p["escalation_window_s"],
            suppression_s=p["suppression_s"],
            reverse_speed_ms=p["reverse_speed_ms"],
            reverse_distance_m=p["reverse_distance_m"],
            angular_speed_rads=p["angular_speed_rads"],
            turn_angle_rad=p["turn_angle_rad"],
            retry_timeout_s=p["retry_timeout_s"],
            clear_hold_s=p["clear_hold_s"],
            max_retries=p["max_retries"],
        )

        # ── State ────────────────────────────────────────────────────────────
        self._latest_obstacle_threat = ThreatEvent()
        self._latest_fall_threat     = ThreatEvent()
        self._latest_bump_threat     = ThreatEvent()
        self._cmd_speed_ms           = 0.0
        self._actual_speed_ms        = 0.0
        self._last_ctrl_t            = time.monotonic()
        self._scan_forward_angle     = p["forward_scan_angle_rad"]
        self._acknowledge_flag       = False

        # ── 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(LaserScan, "/scan",              self._scan_cb,   best_effort)
        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)
        self.create_subscription(Bool,      "/saltybot/emergency_ack", self._ack_cb, reliable)

        # ── Publishers ───────────────────────────────────────────────────────
        self._cmd_out_pub    = self.create_publisher(Twist,  "/saltybot/cmd_vel_out", reliable)
        self._estop_pub      = self.create_publisher(Bool,   "/saltybot/e_stop",      reliable)
        self._beep_pub       = self.create_publisher(Empty,  "/saltybot/alert_beep",  reliable)
        self._flash_pub      = self.create_publisher(Empty,  "/saltybot/alert_flash", reliable)
        self._critical_pub   = self.create_publisher(String, "/saltybot/critical_alert", reliable)

        self._event_pub    = None
        self._recovery_pub = None
        if _MSGS_OK:
            self._event_pub    = self.create_publisher(EmergencyEvent,  "/saltybot/emergency",       reliable)
            self._recovery_pub = self.create_publisher(RecoveryAction, "/saltybot/recovery_action", reliable)

        # ── Timer ────────────────────────────────────────────────────────────
        rate = p["control_rate"]
        self._timer = self.create_timer(1.0 / rate, self._control_cb)
        self.get_logger().info(f"EmergencyNode ready  rate={rate}Hz")

    # ── Parameters ────────────────────────────────────────────────────────────

    def _declare_params(self) -> None:
        self.declare_parameter("control_rate",               20.0)
        self.declare_parameter("odom_topic",                 "/saltybot/rover_odom")
        self.declare_parameter("forward_scan_angle_rad",     0.785)
        self.declare_parameter("stop_distance_m",            0.30)
        self.declare_parameter("critical_distance_m",        0.10)
        self.declare_parameter("min_cmd_speed_ms",           0.05)
        self.declare_parameter("minor_tilt_rad",             0.20)
        self.declare_parameter("major_tilt_rad",             0.35)
        self.declare_parameter("critical_tilt_rad",          0.52)
        self.declare_parameter("floor_drop_m",               0.15)
        self.declare_parameter("stuck_timeout_s",            3.0)
        self.declare_parameter("jerk_threshold_ms3",         8.0)
        self.declare_parameter("critical_jerk_threshold_ms3", 25.0)
        self.declare_parameter("stopped_ms",                 0.03)
        self.declare_parameter("major_count_threshold",      3)
        self.declare_parameter("escalation_window_s",        10.0)
        self.declare_parameter("suppression_s",              1.0)
        self.declare_parameter("reverse_speed_ms",          -0.15)
        self.declare_parameter("reverse_distance_m",         0.30)
        self.declare_parameter("angular_speed_rads",         0.60)
        self.declare_parameter("turn_angle_rad",             1.5708)
        self.declare_parameter("retry_timeout_s",            3.0)
        self.declare_parameter("clear_hold_s",               0.50)
        self.declare_parameter("max_retries",                3)

    def _load_params(self) -> dict:
        g = self.get_parameter
        return {k: g(k).value for k in [
            "control_rate", "odom_topic",
            "forward_scan_angle_rad",
            "stop_distance_m", "critical_distance_m", "min_cmd_speed_ms",
            "minor_tilt_rad", "major_tilt_rad", "critical_tilt_rad", "floor_drop_m",
            "stuck_timeout_s", "jerk_threshold_ms3", "critical_jerk_threshold_ms3",
            "stopped_ms",
            "major_count_threshold", "escalation_window_s", "suppression_s",
            "reverse_speed_ms", "reverse_distance_m",
            "angular_speed_rads", "turn_angle_rad",
            "retry_timeout_s", "clear_hold_s", "max_retries",
        ]}

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

    def _scan_cb(self, msg: LaserScan) -> None:
        # Extract minimum range within forward sector (±forward_scan_angle_rad)
        half = self._scan_forward_angle
        ranges = []
        for i, r in enumerate(msg.ranges):
            angle = msg.angle_min + i * msg.angle_increment
            if abs(angle) <= half and msg.range_min < r < msg.range_max:
                ranges.append(r)
        min_r = min(ranges) if ranges else float("inf")
        self._latest_obstacle_threat = self._obstacle.update(
            min_r, self._cmd_speed_ms, time.monotonic()
        )

    def _imu_cb(self, msg: Imu) -> None:
        now = time.monotonic()
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        pitch, roll = _quaternion_to_pitch_roll(
            msg.orientation.x, msg.orientation.y,
            msg.orientation.z, msg.orientation.w,
        )
        # dt for jerk is approximated; bump detector handles None on first call
        dt = 0.02  # nominal 20 Hz
        self._latest_fall_threat = self._fall.update(pitch, roll, 0.0, now)
        self._latest_bump_threat = self._bump.update(ax, ay, az, dt, now)

    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

    def _ack_cb(self, msg: Bool) -> None:
        if msg.data:
            self._acknowledge_flag = True

    # ── 20 Hz control loop ────────────────────────────────────────────────────

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

        stuck_threat = self._stuck.update(
            self._cmd_speed_ms, self._actual_speed_ms, dt, now
        )

        threat = highest_threat([
            self._latest_obstacle_threat,
            self._latest_fall_threat,
            self._latest_bump_threat,
            stuck_threat,
        ])

        inp = EmergencyInputs(
            threat=threat,
            robot_speed_ms=self._actual_speed_ms,
            acknowledge=self._acknowledge_flag,
        )
        self._acknowledge_flag = False

        out = self._fsm.tick(inp)

        if out.state_changed:
            self.get_logger().info(f"Emergency FSM → {out.state.value}")

        # ── Alert dispatch ────────────────────────────────────────────────────
        if out.alert is not None:
            lvl = out.alert.level
            self.get_logger().warn(out.alert.message)
            if lvl == AlertLevel.MINOR:
                self._beep_pub.publish(Empty())
            elif lvl == AlertLevel.MAJOR:
                self._flash_pub.publish(Empty())
            elif lvl == AlertLevel.CRITICAL:
                self._flash_pub.publish(Empty())
                self._publish_critical_alert(out.alert.message, threat)

        # ── E-stop ───────────────────────────────────────────────────────────
        estop_msg = Bool()
        estop_msg.data = out.e_stop
        self._estop_pub.publish(estop_msg)

        # ── cmd_vel mux ───────────────────────────────────────────────────────
        twist = Twist()
        if out.cmd_override:
            twist.linear.x  = out.cmd_linear
            twist.angular.z = out.cmd_angular
        else:
            twist.linear.x  = self._cmd_speed_ms
        self._cmd_out_pub.publish(twist)

        # ── Status topics ─────────────────────────────────────────────────────
        if self._event_pub is not None:
            self._publish_event(out, threat)
        if self._recovery_pub is not None:
            self._publish_recovery(out)

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

    def _publish_critical_alert(self, message: str, threat: ThreatEvent) -> None:
        msg = String()
        msg.data = json.dumps({
            "severity":   "CRITICAL",
            "threat":     threat.threat_type.value,
            "value":      round(threat.value, 3),
            "detail":     threat.detail,
            "message":    message,
        })
        self._critical_pub.publish(msg)

    def _publish_event(self, out, threat: ThreatEvent) -> None:
        msg = EmergencyEvent()
        msg.stamp        = self.get_clock().now().to_msg()
        msg.state        = out.state.value
        msg.threat_type  = threat.threat_type.value
        msg.severity     = threat.level.name
        msg.threat_value = float(threat.value)
        msg.detail       = threat.detail
        msg.cmd_override = out.cmd_override
        self._event_pub.publish(msg)

    def _publish_recovery(self, out) -> None:
        msg = RecoveryAction()
        msg.stamp        = self.get_clock().now().to_msg()
        msg.action       = self._fsm._recovery.state.value
        msg.retry_count  = out.recovery_retry_count
        msg.progress     = float(out.recovery_progress)
        self._recovery_pub.publish(msg)


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

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


if __name__ == "__main__":
    main()
