"""volume_adjust_node.py — Speech volume auto-adjuster.
Issue #289

Subscribes to /social/speech/energy (linear RMS [0..1] published by
vad_node) and dynamically adjusts TTS output volume via
/saltybot/tts_volume (std_msgs/Float32) so the robot speaks louder in
noisy environments and quieter when ambient noise is low.

Mapping pipeline (runs at publish_rate Hz)
──────────────────────────────────────────
  1. Normalise raw energy into [0, 1] using [noise_floor, noise_ceil]:
       t = clamp((energy - noise_floor) / (noise_ceil - noise_floor), 0, 1)
  2. Apply power curve:
       t_curved = t ^ curve_gamma
       gamma < 1 → concave  (strong response at low noise, gentle at top)
       gamma = 1 → linear
       gamma > 1 → convex   (gentle response at low noise, strong at top)
  3. Map to volume:
       vol_target = min_volume + (max_volume - min_volume) * t_curved
  4. Exponential moving-average smoothing:
       vol_smoothed = alpha * vol_target + (1-alpha) * vol_smoothed
  5. If no energy sample received for stale_timeout_s → hold last value.

Parameters
──────────
  energy_topic   (str,   "/social/speech/energy")
  volume_topic   (str,   "/saltybot/tts_volume")
  min_volume     (float, 0.5)   lowest TTS volume [0..1]
  max_volume     (float, 1.0)   highest TTS volume [0..1]
  noise_floor    (float, 0.001) energy ≤ this maps to min_volume
  noise_ceil     (float, 0.10)  energy ≥ this maps to max_volume
  curve_gamma    (float, 0.5)   power-curve exponent
  smoothing_alpha(float, 0.1)   EMA weight (0=frozen, 1=instant)
  publish_rate   (float, 5.0)   Hz
  stale_timeout_s(float, 5.0)   seconds before energy considered stale
"""

from __future__ import annotations

import time
import threading

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from std_msgs.msg import Float32


# ── Pure helpers ───────────────────────────────────────────────────────────────

def clamp(v: float, lo: float, hi: float) -> float:
    return max(lo, min(hi, v))


def normalize_energy(energy: float, noise_floor: float, noise_ceil: float) -> float:
    """Map raw linear energy into [0, 1] within [noise_floor, noise_ceil]."""
    span = noise_ceil - noise_floor
    if span <= 0.0:
        return 1.0 if energy >= noise_ceil else 0.0
    return clamp((energy - noise_floor) / span, 0.0, 1.0)


def apply_curve(t: float, gamma: float) -> float:
    """Apply power-curve shaping.  t ∈ [0,1], gamma > 0."""
    if gamma <= 0.0:
        return 1.0
    return t ** gamma


def compute_target_volume(energy: float,
                          noise_floor: float,
                          noise_ceil: float,
                          gamma: float,
                          min_volume: float,
                          max_volume: float) -> float:
    """Return target volume [min_volume, max_volume] for a given energy."""
    t = normalize_energy(energy, noise_floor, noise_ceil)
    t_curved = apply_curve(t, gamma)
    return min_volume + (max_volume - min_volume) * t_curved


def ema_update(current: float, target: float, alpha: float) -> float:
    """Single-step exponential moving average."""
    alpha = clamp(alpha, 0.0, 1.0)
    return alpha * target + (1.0 - alpha) * current


# ── ROS2 node ──────────────────────────────────────────────────────────────────

class VolumeAdjustNode(Node):
    """Dynamically adjusts TTS volume from ambient energy measurements."""

    def __init__(self) -> None:
        super().__init__("volume_adjust_node")

        self.declare_parameter("energy_topic",    "/social/speech/energy")
        self.declare_parameter("volume_topic",    "/saltybot/tts_volume")
        self.declare_parameter("min_volume",       0.5)
        self.declare_parameter("max_volume",       1.0)
        self.declare_parameter("noise_floor",      0.001)
        self.declare_parameter("noise_ceil",       0.10)
        self.declare_parameter("curve_gamma",      0.5)
        self.declare_parameter("smoothing_alpha",  0.1)
        self.declare_parameter("publish_rate",     5.0)
        self.declare_parameter("stale_timeout_s",  5.0)

        energy_topic    = self.get_parameter("energy_topic").value
        volume_topic    = self.get_parameter("volume_topic").value
        self._min_vol   = self.get_parameter("min_volume").value
        self._max_vol   = self.get_parameter("max_volume").value
        self._floor     = self.get_parameter("noise_floor").value
        self._ceil      = self.get_parameter("noise_ceil").value
        self._gamma     = self.get_parameter("curve_gamma").value
        self._alpha     = self.get_parameter("smoothing_alpha").value
        self._stale_t   = self.get_parameter("stale_timeout_s").value
        publish_rate    = self.get_parameter("publish_rate").value

        # Start at min_volume; smoothed toward target as data arrives
        self._current_vol: float = self._min_vol
        self._latest_energy: float = 0.0
        self._last_energy_t: float = 0.0
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._vol_pub = self.create_publisher(Float32, volume_topic, qos)
        self._energy_sub = self.create_subscription(
            Float32, energy_topic, self._on_energy, qos
        )
        self._timer = self.create_timer(1.0 / publish_rate, self._publish_cb)

        self.get_logger().info(
            f"VolumeAdjustNode ready "
            f"(vol=[{self._min_vol},{self._max_vol}], "
            f"floor={self._floor}, ceil={self._ceil}, "
            f"gamma={self._gamma}, alpha={self._alpha})"
        )

    # ── Subscription ───────────────────────────────────────────────────────

    def _on_energy(self, msg: Float32) -> None:
        with self._lock:
            self._latest_energy = float(msg.data)
            self._last_energy_t = time.monotonic()

    # ── Timer / publish ────────────────────────────────────────────────────

    def _publish_cb(self) -> None:
        now = time.monotonic()
        with self._lock:
            energy     = self._latest_energy
            last_t     = self._last_energy_t
            fresh      = last_t > 0.0 and (now - last_t) < self._stale_t

        if fresh:
            target = compute_target_volume(
                energy, self._floor, self._ceil,
                self._gamma, self._min_vol, self._max_vol,
            )
            with self._lock:
                self._current_vol = ema_update(
                    self._current_vol, target, self._alpha
                )

        msg = Float32()
        with self._lock:
            msg.data = float(self._current_vol)
        self._vol_pub.publish(msg)

    # ── Property (for testing) ─────────────────────────────────────────────

    @property
    def current_volume(self) -> float:
        with self._lock:
            return self._current_vol


def main(args=None) -> None:
    rclpy.init(args=args)
    node = VolumeAdjustNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()
