"""
behavior_adapter_node.py — Adapt robot speed and personality from scene context.

Subscribes:
  /social/scene/room_type    RoomClassification   — current room
  /social/scene/hazards      SceneObjectArray     — active hazards

Publishes:
  /social/scene/behavior_hint   BehaviorHint   (on change + 1 Hz heartbeat)

Behavior rules (speed_limit_mps, turn_limit_rads, personality_mode):
  Baseline:          1.0,  1.5,  'efficient'
  living_room:       0.6,  1.2,  'gentle'
  kitchen:           0.5,  1.0,  'careful'
  hallway:           0.8,  1.5,  'efficient'
  bedroom:           0.4,  0.8,  'gentle'
  garage:            1.2,  2.0,  'efficient'
  outdoor:           1.5,  2.0,  'active'
  office:            0.5,  1.0,  'careful'

Hazard overrides (highest severity wins):
  HAZARD_STAIRS:     0.1,  0.5,  'cautious',  alert=True
  HAZARD_DROP:       0.1,  0.5,  'cautious',  alert=True
  HAZARD_WET_FLOOR:  0.3,  0.8,  'careful',   alert=True
  HAZARD_GLASS_DOOR: 0.2,  0.5,  'cautious',  alert=True
  HAZARD_PET:        0.3,  0.8,  'gentle',    pet_nearby=True
"""

import time

import rclpy
from rclpy.node import Node

from saltybot_scene_msgs.msg import RoomClassification, SceneObjectArray, BehaviorHint

from .hazard_classifier import (
    HAZARD_NONE, HAZARD_STAIRS, HAZARD_DROP, HAZARD_WET_FLOOR,
    HAZARD_GLASS_DOOR, HAZARD_PET
)
from .room_classifier import ROOM_NAMES

# ── Room speed/personality profiles ──────────────────────────────────────────
# (speed_mps, turn_rads, personality)
_ROOM_PROFILE = {
    0: (1.0, 1.5, 'efficient'),   # unknown — baseline
    1: (0.6, 1.2, 'gentle'),      # living_room
    2: (0.5, 1.0, 'careful'),     # kitchen
    3: (0.8, 1.5, 'efficient'),   # hallway
    4: (0.4, 0.8, 'gentle'),      # bedroom
    5: (1.2, 2.0, 'efficient'),   # garage
    6: (1.5, 2.0, 'active'),      # outdoor
    7: (0.5, 1.0, 'careful'),     # office
}

# ── Hazard override profiles ──────────────────────────────────────────────────
# (speed_mps, turn_rads, personality, alert_reason)
_HAZARD_OVERRIDE = {
    HAZARD_STAIRS:     (0.10, 0.5, 'cautious',  'stairs ahead'),
    HAZARD_DROP:       (0.10, 0.5, 'cautious',  'floor drop ahead'),
    HAZARD_WET_FLOOR:  (0.30, 0.8, 'careful',   'wet floor'),
    HAZARD_GLASS_DOOR: (0.20, 0.5, 'cautious',  'glass door ahead'),
    HAZARD_PET:        (0.30, 0.8, 'gentle',    'pet nearby'),
}

# Severity order — highest number wins
_HAZARD_SEVERITY = {
    HAZARD_NONE:      0,
    HAZARD_PET:       1,
    HAZARD_WET_FLOOR: 2,
    HAZARD_GLASS_DOOR: 3,
    HAZARD_DROP:      4,
    HAZARD_STAIRS:    4,
}


class BehaviorAdapterNode(Node):

    def __init__(self):
        super().__init__('behavior_adapter')

        self._current_room:    RoomClassification | None = None
        self._active_hazards:  list                      = []
        self._last_hint:       BehaviorHint | None       = None
        self._last_publish_t:  float                     = 0.0

        self.create_subscription(
            RoomClassification, '/social/scene/room_type', self._on_room, 10
        )
        self.create_subscription(
            SceneObjectArray, '/social/scene/hazards', self._on_hazards, 10
        )

        self._hint_pub = self.create_publisher(
            BehaviorHint, '/social/scene/behavior_hint', 10
        )

        # Heartbeat + hazard expiry timer
        self.create_timer(1.0, self._tick)

        self.get_logger().info('behavior_adapter ready')

    def _on_room(self, msg: RoomClassification) -> None:
        if self._current_room is None or msg.room_type != self._current_room.room_type:
            self.get_logger().info(
                f'[scene] room changed → {msg.room_name} '
                f'(conf={msg.confidence:.0%})'
            )
        self._current_room = msg
        self._publish_hint()

    def _on_hazards(self, msg: SceneObjectArray) -> None:
        self._active_hazards = list(msg.objects)
        self._publish_hint()

    def _tick(self) -> None:
        # Expire hazards older than 2s (no new SceneObjectArray received)
        self._active_hazards = []
        # Heartbeat
        self._publish_hint()

    def _publish_hint(self) -> None:
        hint = self._build_hint()
        # Only publish if something changed or 1s heartbeat
        now = time.monotonic()
        changed = (self._last_hint is None or
                   self._hint_changed(hint, self._last_hint))
        if not changed and now - self._last_publish_t < 1.0:
            return
        self._hint_pub.publish(hint)
        self._last_hint     = hint
        self._last_publish_t = now

    def _build_hint(self) -> BehaviorHint:
        hint = BehaviorHint()
        hint.header.stamp = self.get_clock().now().to_msg()

        # ── Room baseline ──────────────────────────────────────────────────
        room_type = self._current_room.room_type if self._current_room else 0
        speed, turn, personality = _ROOM_PROFILE.get(room_type, (1.0, 1.5, 'efficient'))
        hint.room_name       = ROOM_NAMES.get(room_type, 'unknown')
        hint.speed_limit_mps = speed
        hint.turn_limit_rads = turn
        hint.personality_mode = personality

        # ── Hazard override ────────────────────────────────────────────────
        best_severity  = 0
        best_hazard    = HAZARD_NONE
        for obj in self._active_hazards:
            sev = _HAZARD_SEVERITY.get(obj.hazard_type, 0)
            if sev > best_severity:
                best_severity = sev
                best_hazard   = obj.hazard_type

        if best_hazard != HAZARD_NONE:
            ov_speed, ov_turn, ov_personality, ov_reason = _HAZARD_OVERRIDE[best_hazard]
            # Take the more conservative limit
            hint.speed_limit_mps  = min(hint.speed_limit_mps, ov_speed)
            hint.turn_limit_rads  = min(hint.turn_limit_rads, ov_turn)
            hint.personality_mode = ov_personality
            hint.hazard_alert     = True
            hint.alert_reason     = ov_reason
            hint.hazard_type      = best_hazard
            hint.pet_nearby       = (best_hazard == HAZARD_PET)
        else:
            hint.hazard_alert = False
            hint.hazard_type  = HAZARD_NONE

        return hint

    @staticmethod
    def _hint_changed(a: BehaviorHint, b: BehaviorHint) -> bool:
        return (
            abs(a.speed_limit_mps - b.speed_limit_mps) > 0.05
            or a.personality_mode != b.personality_mode
            or a.hazard_alert != b.hazard_alert
            or a.room_name != b.room_name
        )


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


if __name__ == '__main__':
    main()
