#!/usr/bin/env python3
"""Night Mode Controller for SaltyBot.

Detects ambient light from RealSense IR or phone sensors and automatically
switches to stealth mode with reduced motor speed, IR tracking, dimmed LEDs,
and face-only TTS. Supports manual override via voice/gamepad.

Published topics:
  /saltybot/night_mode (std_msgs/String) - JSON night mode status
  /saltybot/motor_speed_override (std_msgs/Float32) - Motor speed scale (0.0-1.0)
  /saltybot/tts_disable (std_msgs/Bool) - TTS disable flag
  /saltybot/face_control (std_msgs/String) - Face display settings JSON
  /saltybot/led_brightness_override (std_msgs/Float32) - LED brightness

Subscribed topics:
  /saltybot/light_level (std_msgs/String) - Ambient light level in lux
  /saltybot/voice/command (std_msgs/String) - Voice commands
  /saltybot/gamepad/input (std_msgs/String) - Gamepad input
"""

import json
import time
from collections import deque
from dataclasses import dataclass
from typing import Dict, Optional, Deque

import yaml
import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Float32, Bool


@dataclass
class NightModeState:
    """Night mode state information."""
    active: bool
    light_level: float  # Lux
    transition_in_progress: bool
    transition_progress: float  # 0.0-1.0
    manual_override: bool
    override_mode: Optional[str]  # 'night', 'day', or None


class LightSensor:
    """Light level sensor aggregator."""

    def __init__(self, smoothing_window: int = 5):
        """Initialize light sensor.
        
        Args:
            smoothing_window: Number of samples to average
        """
        self.smoothing_window = smoothing_window
        self.samples: Deque[float] = deque(maxlen=smoothing_window)
        self.current_light = 1000.0  # Start assuming daytime (high light)

    def update(self, light_level: float):
        """Update with new light level reading."""
        self.samples.append(light_level)
        self.current_light = sum(self.samples) / len(self.samples)

    def get_light_level(self) -> float:
        """Get smoothed light level."""
        return self.current_light if self.samples else self.current_light


class TransitionManager:
    """Manages smooth transitions between night and day modes."""

    def __init__(self, fade_duration: float = 1.0):
        """Initialize transition manager.
        
        Args:
            fade_duration: Transition duration in seconds
        """
        self.fade_duration = fade_duration
        self.transition_start = None
        self.transition_type = None  # 'to_night' or 'to_day'

    def start_transition(self, target_mode: str):
        """Start a transition to new mode.
        
        Args:
            target_mode: 'night' or 'day'
        """
        self.transition_start = time.time()
        self.transition_type = f"to_{target_mode}"

    def get_progress(self) -> float:
        """Get transition progress (0.0-1.0)."""
        if self.transition_start is None:
            return 0.0

        elapsed = time.time() - self.transition_start
        progress = min(1.0, elapsed / self.fade_duration)

        if progress >= 1.0:
            self.transition_start = None
            self.transition_type = None

        return progress

    def is_transitioning(self) -> bool:
        """Check if transition in progress."""
        return self.transition_start is not None

    def apply_transition(self, day_value: float, night_value: float) -> float:
        """Apply transition interpolation.
        
        Args:
            day_value: Value for day mode
            night_value: Value for night mode
            
        Returns:
            Interpolated value
        """
        progress = self.get_progress()
        if not self.is_transitioning():
            return night_value if self.transition_type == 'to_night' else day_value

        if 'night' in self.transition_type:
            return day_value + (night_value - day_value) * progress
        else:
            return night_value + (day_value - night_value) * progress


class NightModeNode(Node):
    """ROS2 node for night mode control."""

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

        # Load config
        self.declare_parameter("config_file", "night_mode_config.yaml")
        config_path = self.get_parameter("config_file").value
        self.config = self._load_config(config_path)

        # State tracking
        self.light_sensor = LightSensor(
            self.config["light_sensor"]["smoothing_window"]
        )
        self.transition_mgr = TransitionManager(
            self.config["transitions"]["fade_duration"]
        )
        self.night_mode_active = False
        self.manual_override = False
        self.override_target = None
        self.last_update = time.time()

        # Subscribers
        self.create_subscription(String, "/saltybot/light_level", self._light_cb, 10)
        self.create_subscription(String, "/saltybot/voice/command", self._voice_cb, 10)
        self.create_subscription(String, "/saltybot/gamepad/input", self._gamepad_cb, 10)

        # Publishers
        self.night_mode_pub = self.create_publisher(
            String, "/saltybot/night_mode", 10
        )
        self.motor_override_pub = self.create_publisher(
            Float32, "/saltybot/motor_speed_override", 10
        )
        self.tts_disable_pub = self.create_publisher(
            Bool, "/saltybot/tts_disable", 10
        )
        self.face_control_pub = self.create_publisher(
            String, "/saltybot/face_control", 10
        )
        self.led_override_pub = self.create_publisher(
            Float32, "/saltybot/led_brightness_override", 10
        )

        # Timer for main loop
        self.timer = self.create_timer(0.1, self._update_night_mode)

        self.get_logger().info("Night Mode Controller initialized")

    def _load_config(self, config_path: str) -> Dict:
        """Load YAML configuration."""
        try:
            with open(config_path) as f:
                return yaml.safe_load(f)
        except FileNotFoundError:
            self.get_logger().warn(f"Config not found: {config_path}")
            return self._default_config()

    def _default_config(self) -> Dict:
        """Return default configuration."""
        return {
            "light_sensor": {
                "threshold_lux": 50,
                "hysteresis": 5,
                "smoothing_window": 5,
            },
            "night_mode": {
                "enabled": True,
                "motor_speed_percent": 50,
                "disable_tts": True,
                "led_brightness_percent": 5,
                "face_brightness_percent": 30,
                "ir_tracking_only": True,
                "stealth_led": {
                    "color": [0, 20, 80],
                    "pattern": "slow_dim",
                    "speed": 0.3,
                    "enabled": True,
                },
            },
            "overrides": {
                "voice_control": True,
                "gamepad_control": True,
                "gamepad_button": "y",
            },
            "transitions": {
                "fade_duration": 1.0,
                "motor_ramp_time": 2.0,
                "led_fade_time": 0.5,
            },
            "publications": {
                "night_mode_state": "/saltybot/night_mode",
                "motor_override": "/saltybot/motor_speed_override",
                "tts_override": "/saltybot/tts_disable",
                "face_control": "/saltybot/face_control",
                "led_override": "/saltybot/led_brightness_override",
            },
        }

    def _light_cb(self, msg: String):
        """Handle light level sensor data."""
        try:
            data = json.loads(msg.data)
            light_level = data.get("lux", 1000.0)
            self.light_sensor.update(light_level)
        except (json.JSONDecodeError, KeyError):
            pass

    def _voice_cb(self, msg: String):
        """Handle voice commands for manual override."""
        try:
            data = json.loads(msg.data)
            command = data.get("command", "").lower()

            if "night mode" in command or "stealth" in command:
                if "on" in command or "enable" in command:
                    self._set_manual_override("night")
                elif "off" in command or "disable" in command:
                    self._set_manual_override("day")
                elif "toggle" in command:
                    self._toggle_manual_override()
        except json.JSONDecodeError:
            pass

    def _gamepad_cb(self, msg: String):
        """Handle gamepad input for manual override."""
        try:
            data = json.loads(msg.data)
            button = data.get("button", "")
            pressed = data.get("pressed", False)

            if button == self.config["overrides"]["gamepad_button"] and pressed:
                self._toggle_manual_override()
        except json.JSONDecodeError:
            pass

    def _set_manual_override(self, mode: str):
        """Set manual mode override.
        
        Args:
            mode: 'night', 'day', or None for auto
        """
        if mode in ["night", "day"]:
            self.manual_override = True
            self.override_target = mode
            self.get_logger().info(f"Manual override: {mode} mode")
        else:
            self.manual_override = False
            self.override_target = None
            self.get_logger().info("Manual override disabled (auto mode)")

    def _toggle_manual_override(self):
        """Toggle manual override mode."""
        if not self.manual_override:
            self._set_manual_override("night" if not self.night_mode_active else "day")
        else:
            self._set_manual_override(None)

    def _determine_night_mode(self) -> bool:
        """Determine if night mode should be active.
        
        Returns:
            True if night mode should be active, False otherwise
        """
        if self.manual_override:
            return self.override_target == "night"

        light_level = self.light_sensor.get_light_level()
        threshold = self.config["light_sensor"]["threshold_lux"]
        hysteresis = self.config["light_sensor"]["hysteresis"]

        if self.night_mode_active:
            # Hysteresis: need more light to exit night mode
            return light_level < (threshold + hysteresis)
        else:
            # Normal threshold: enter night mode
            return light_level < threshold

    def _get_motor_speed_scale(self) -> float:
        """Get motor speed scaling factor (0.0-1.0)."""
        if not self.night_mode_active:
            return 1.0

        target_speed = self.config["night_mode"]["motor_speed_percent"] / 100.0

        if self.transition_mgr.is_transitioning():
            return self.transition_mgr.apply_transition(1.0, target_speed)

        return target_speed

    def _get_led_brightness_scale(self) -> float:
        """Get LED brightness scaling factor (0.0-1.0)."""
        if not self.night_mode_active:
            return 1.0

        target_brightness = self.config["night_mode"]["led_brightness_percent"] / 100.0

        if self.transition_mgr.is_transitioning():
            return self.transition_mgr.apply_transition(1.0, target_brightness)

        return target_brightness

    def _get_face_brightness_scale(self) -> float:
        """Get face brightness scaling factor (0.0-1.0)."""
        if not self.night_mode_active:
            return 1.0

        target_brightness = self.config["night_mode"]["face_brightness_percent"] / 100.0

        if self.transition_mgr.is_transitioning():
            return self.transition_mgr.apply_transition(1.0, target_brightness)

        return target_brightness

    def _update_night_mode(self):
        """Update night mode state and publish overrides."""
        # Determine mode
        should_be_night = self._determine_night_mode()

        # Handle mode transition
        if should_be_night != self.night_mode_active:
            self.night_mode_active = should_be_night
            target = "night" if should_be_night else "day"
            self.transition_mgr.start_transition(target)
            self.get_logger().info(f"Night mode: {should_be_night}")

        # Publish night mode state
        state = {
            "active": self.night_mode_active,
            "light_level": round(self.light_sensor.get_light_level(), 1),
            "transitioning": self.transition_mgr.is_transitioning(),
            "manual_override": self.manual_override,
            "threshold_lux": self.config["light_sensor"]["threshold_lux"],
        }
        self.night_mode_pub.publish(String(data=json.dumps(state)))

        # Publish motor speed override
        motor_scale = self._get_motor_speed_scale()
        self.motor_override_pub.publish(Float32(data=motor_scale))

        # Publish TTS disable
        tts_disabled = self.night_mode_active and self.config["night_mode"]["disable_tts"]
        self.tts_disable_pub.publish(Bool(data=tts_disabled))

        # Publish face control settings
        face_config = {
            "display_only": self.night_mode_active,
            "brightness_scale": self._get_face_brightness_scale(),
            "contrast_boost": self.night_mode_active,
        }
        self.face_control_pub.publish(String(data=json.dumps(face_config)))

        # Publish LED brightness override
        led_scale = self._get_led_brightness_scale()
        self.led_override_pub.publish(Float32(data=led_scale))


def main(args=None):
    """Main entry point."""
    rclpy.init(args=args)
    node = NightModeNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    main()
