"""
camera_power_node.py — Adaptive camera power mode manager (Issue #375).

Subscribes to speed, scenario, and battery level inputs, runs the
CameraPowerFSM, and publishes camera enable/disable commands at 2 Hz.

Subscribes
----------
  /saltybot/speed       std_msgs/Float32   robot speed in m/s
  /saltybot/scenario    std_msgs/String    operating scenario label
  /saltybot/battery_pct std_msgs/Float32   battery charge level 0–100 %

Publishes
---------
  /saltybot/camera_mode          saltybot_scene_msgs/CameraPowerMode  (2 Hz)
  /saltybot/camera_cmd/front     std_msgs/Bool  — front CSI enable
  /saltybot/camera_cmd/rear      std_msgs/Bool  — rear  CSI enable
  /saltybot/camera_cmd/left      std_msgs/Bool  — left  CSI enable
  /saltybot/camera_cmd/right     std_msgs/Bool  — right CSI enable
  /saltybot/camera_cmd/realsense std_msgs/Bool  — D435i enable
  /saltybot/camera_cmd/lidar     std_msgs/Bool  — LIDAR enable
  /saltybot/camera_cmd/uwb       std_msgs/Bool  — UWB enable
  /saltybot/camera_cmd/webcam    std_msgs/Bool  — C920 webcam enable

Parameters
----------
  rate_hz             float   2.0    FSM evaluation and publish rate
  downgrade_hold_s    float   5.0    Seconds before a downgrade is applied
  idle_to_social_s    float  30.0    Idle time before AWARE→SOCIAL
  battery_low_pct     float  20.0    Battery threshold for AWARE cap
  initial_mode        int     0      Starting mode (0=SLEEP … 4=FULL)
"""

from __future__ import annotations

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

from std_msgs.msg import Bool, Float32, String
from saltybot_scene_msgs.msg import CameraPowerMode

from ._camera_power_manager import (
    CameraMode,
    CameraPowerFSM,
    Scenario,
    MODE_SENSORS,
)


_RELIABLE_LATCHED = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
)
_RELIABLE = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10,
)
_SENSOR_QOS = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    history=HistoryPolicy.KEEP_LAST,
    depth=5,
)

# Camera command topic suffixes and their ActiveSensors field name
_CAM_TOPICS = [
    ('front',     'csi_front'),
    ('rear',      'csi_rear'),
    ('left',      'csi_left'),
    ('right',     'csi_right'),
    ('realsense', 'realsense'),
    ('lidar',     'lidar'),
    ('uwb',       'uwb'),
    ('webcam',    'webcam'),
]


class CameraPowerNode(Node):

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

        # ── Parameters ──────────────────────────────────────────────────────
        self.declare_parameter('rate_hz',          2.0)
        self.declare_parameter('downgrade_hold_s', 5.0)
        self.declare_parameter('idle_to_social_s', 30.0)
        self.declare_parameter('battery_low_pct',  20.0)
        self.declare_parameter('initial_mode',     0)

        p = self.get_parameter
        rate_hz           = max(float(p('rate_hz').value),          0.1)
        downgrade_hold_s  = max(float(p('downgrade_hold_s').value), 0.0)
        idle_to_social_s  = max(float(p('idle_to_social_s').value), 0.0)
        battery_low_pct   = float(p('battery_low_pct').value)
        initial_mode      = int(p('initial_mode').value)

        # ── FSM ──────────────────────────────────────────────────────────────
        self._fsm = CameraPowerFSM(
            downgrade_hold_s = downgrade_hold_s,
            idle_to_social_s = idle_to_social_s,
            battery_low_pct  = battery_low_pct,
        )
        try:
            self._fsm.reset(CameraMode(initial_mode))
        except ValueError:
            self._fsm.reset(CameraMode.SLEEP)

        # ── Input state ──────────────────────────────────────────────────────
        self._speed_mps:   float = 0.0
        self._scenario:    str   = Scenario.UNKNOWN
        self._battery_pct: float = 100.0

        # ── Subscribers ──────────────────────────────────────────────────────
        self._speed_sub = self.create_subscription(
            Float32, '/saltybot/speed',
            lambda m: setattr(self, '_speed_mps', max(0.0, float(m.data))),
            _SENSOR_QOS,
        )
        self._scenario_sub = self.create_subscription(
            String, '/saltybot/scenario',
            lambda m: setattr(self, '_scenario', str(m.data)),
            _RELIABLE,
        )
        self._battery_sub = self.create_subscription(
            Float32, '/saltybot/battery_pct',
            lambda m: setattr(self, '_battery_pct',
                              max(0.0, min(100.0, float(m.data)))),
            _RELIABLE,
        )

        # ── Publishers ───────────────────────────────────────────────────────
        self._mode_pub = self.create_publisher(
            CameraPowerMode, '/saltybot/camera_mode', _RELIABLE_LATCHED,
        )
        self._cam_pubs: dict[str, rclpy.publisher.Publisher] = {}
        for suffix, _ in _CAM_TOPICS:
            self._cam_pubs[suffix] = self.create_publisher(
                Bool, f'/saltybot/camera_cmd/{suffix}', _RELIABLE_LATCHED,
            )

        # ── Timer ────────────────────────────────────────────────────────────
        self._timer = self.create_timer(1.0 / rate_hz, self._step)
        self._prev_mode: CameraMode | None = None

        self.get_logger().info(
            f'camera_power_node ready — '
            f'rate={rate_hz:.1f}Hz, '
            f'downgrade_hold={downgrade_hold_s}s, '
            f'idle_to_social={idle_to_social_s}s, '
            f'battery_low={battery_low_pct}%'
        )

    # ── Step callback ─────────────────────────────────────────────────────────

    def _step(self) -> None:
        decision = self._fsm.update(
            speed_mps   = self._speed_mps,
            scenario    = self._scenario,
            battery_pct = self._battery_pct,
        )

        # Log transitions
        if decision.mode != self._prev_mode:
            prev_name = self._prev_mode.label if self._prev_mode else 'INIT'
            self.get_logger().info(
                f'Camera mode: {prev_name} → {decision.mode.label}  '
                f'(speed={self._speed_mps:.2f}m/s  '
                f'scenario={self._scenario}  '
                f'battery={self._battery_pct:.0f}%)'
            )
            self._prev_mode = decision.mode

        # Publish CameraPowerMode message
        msg = CameraPowerMode()
        msg.header.stamp    = self.get_clock().now().to_msg()
        msg.header.frame_id = ''
        msg.mode            = int(decision.mode)
        msg.mode_name       = decision.mode.label
        s = decision.sensors
        msg.csi_front        = s.csi_front
        msg.csi_rear         = s.csi_rear
        msg.csi_left         = s.csi_left
        msg.csi_right        = s.csi_right
        msg.realsense        = s.realsense
        msg.lidar            = s.lidar
        msg.uwb              = s.uwb
        msg.webcam           = s.webcam
        msg.trigger_speed_mps = float(decision.trigger_speed_mps)
        msg.trigger_scenario  = decision.trigger_scenario
        msg.scenario_override = decision.scenario_override
        self._mode_pub.publish(msg)

        # Publish per-camera Bool commands
        for suffix, field in _CAM_TOPICS:
            enabled = bool(getattr(s, field))
            b = Bool()
            b.data = enabled
            self._cam_pubs[suffix].publish(b)


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


if __name__ == '__main__':
    main()
