"""face_track_servo_node.py — Face-tracking head servo controller.
Issue #279

Subscribes to /social/faces/detected, picks the closest face (largest
bounding-box area as a proximity proxy), computes pan/tilt angular error
relative to the image centre, and drives two PID controllers to produce
smooth servo position commands published on /saltybot/head_pan and
/saltybot/head_tilt (std_msgs/Float32, degrees from neutral).

Coordinate convention
─────────────────────
  bbox_x/y/w/h  : normalised [0, 1] in image space
  face centre   : cx = bbox_x + bbox_w/2 ,  cy = bbox_y + bbox_h/2
  image centre  : (0.5, 0.5)
  pan  error    : (cx - 0.5) * fov_h_deg   (+ve → face right of centre)
  tilt error    : (cy - 0.5) * fov_v_deg   (+ve → face below centre)

PID design (velocity / incremental)
────────────────────────────────────
  velocity (°/s) = Kp·e + Ki·∫e dt + Kd·de/dt
  servo_angle  += velocity · dt
  servo_angle   = clamp(servo_angle, ±limit_deg)

When no face is seen for more than ``lost_timeout_s`` seconds the PIDs
are reset and the servo commands return toward 0° at ``return_rate_deg_s``.

Parameters
──────────
  kp_pan            (float, 1.5)    pan proportional gain   (°/s per °)
  ki_pan            (float, 0.1)    pan integral gain
  kd_pan            (float, 0.05)   pan derivative gain
  kp_tilt           (float, 1.2)    tilt proportional gain
  ki_tilt           (float, 0.1)    tilt integral gain
  kd_tilt           (float, 0.04)   tilt derivative gain
  fov_h_deg         (float, 60.0)   camera horizontal FOV (degrees)
  fov_v_deg         (float, 45.0)   camera vertical FOV (degrees)
  pan_limit_deg     (float, 90.0)   mechanical pan limit ±
  tilt_limit_deg    (float, 30.0)   mechanical tilt limit ±
  pan_vel_limit     (float, 45.0)   max pan rate (°/s)
  tilt_vel_limit    (float, 30.0)   max tilt rate (°/s)
  windup_limit      (float, 15.0)   integral anti-windup clamp (°·s)
  dead_zone         (float, 0.02)   normalised dead zone (fraction of frame)
  control_rate      (float, 20.0)   control loop Hz
  lost_timeout_s    (float, 1.5)    seconds before face considered lost
  return_rate_deg_s (float, 10.0)   return-to-centre rate when no face (°/s)
  faces_topic       (str)           default "/social/faces/detected"
"""

from __future__ import annotations

import math
import time
import threading
from typing import Optional

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

try:
    from saltybot_social_msgs.msg import FaceDetectionArray
    _MSGS = True
except ImportError:
    _MSGS = False


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

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


def bbox_area(face) -> float:
    """Bounding-box area as a proximity proxy (larger ≈ closer)."""
    return float(face.bbox_w) * float(face.bbox_h)


def pick_closest_face(faces):
    """Return the face with the largest bbox area; None if list is empty."""
    if not faces:
        return None
    return max(faces, key=bbox_area)


def face_image_error(face, fov_h_deg: float, fov_v_deg: float):
    """Return (pan_error_deg, tilt_error_deg) for a FaceDetection.

    Positive pan  → face is right of image centre.
    Positive tilt → face is below image centre.
    """
    cx = float(face.bbox_x) + float(face.bbox_w) / 2.0
    cy = float(face.bbox_y) + float(face.bbox_h) / 2.0
    pan_err  = (cx - 0.5) * fov_h_deg
    tilt_err = (cy - 0.5) * fov_v_deg
    return pan_err, tilt_err


# ── PID controller ─────────────────────────────────────────────────────────────

class PIDController:
    """Incremental (velocity-output) PID with anti-windup.

    Output units: degrees/second (servo angular velocity).
    Integrate externally: servo_angle += pid.update(error, dt) * dt
    """

    def __init__(self, kp: float, ki: float, kd: float,
                 vel_limit: float, windup_limit: float) -> None:
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.vel_limit    = vel_limit
        self.windup_limit = windup_limit
        self._integral    = 0.0
        self._prev_error  = 0.0
        self._first       = True

    def update(self, error: float, dt: float) -> float:
        """Return velocity command (°/s).  Call every control tick."""
        if dt <= 0.0:
            return 0.0

        self._integral += error * dt
        self._integral  = clamp(self._integral, -self.windup_limit,
                                self.windup_limit)

        if self._first:
            derivative      = 0.0
            self._first     = False
        else:
            derivative = (error - self._prev_error) / dt

        self._prev_error = error
        output = (self.kp * error
                  + self.ki * self._integral
                  + self.kd * derivative)
        return clamp(output, -self.vel_limit, self.vel_limit)

    def reset(self) -> None:
        self._integral   = 0.0
        self._prev_error = 0.0
        self._first      = True


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

class FaceTrackServoNode(Node):
    """Smooth PID face-tracking servo controller."""

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

        # Declare parameters
        self.declare_parameter("kp_pan",            1.5)
        self.declare_parameter("ki_pan",            0.1)
        self.declare_parameter("kd_pan",            0.05)
        self.declare_parameter("kp_tilt",           1.2)
        self.declare_parameter("ki_tilt",           0.1)
        self.declare_parameter("kd_tilt",           0.04)
        self.declare_parameter("fov_h_deg",         60.0)
        self.declare_parameter("fov_v_deg",         45.0)
        self.declare_parameter("pan_limit_deg",     90.0)
        self.declare_parameter("tilt_limit_deg",    30.0)
        self.declare_parameter("pan_vel_limit",     45.0)
        self.declare_parameter("tilt_vel_limit",    30.0)
        self.declare_parameter("windup_limit",      15.0)
        self.declare_parameter("dead_zone",         0.02)
        self.declare_parameter("control_rate",      20.0)
        self.declare_parameter("lost_timeout_s",    1.5)
        self.declare_parameter("return_rate_deg_s", 10.0)
        self.declare_parameter("faces_topic",       "/social/faces/detected")

        self._reload_params()

        # Servo state
        self._pan_cmd  = 0.0
        self._tilt_cmd = 0.0
        self._last_face_t: float = 0.0
        self._latest_face = None
        self._lock = threading.Lock()

        qos = QoSProfile(depth=10)
        self._pan_pub  = self.create_publisher(Float32, "/saltybot/head_pan",  qos)
        self._tilt_pub = self.create_publisher(Float32, "/saltybot/head_tilt", qos)

        faces_topic = self.get_parameter("faces_topic").value
        if _MSGS:
            self._faces_sub = self.create_subscription(
                FaceDetectionArray, faces_topic, self._on_faces, qos
            )
        else:
            self.get_logger().warn(
                "saltybot_social_msgs not available — node passive (no subscription)"
            )

        rate = self.get_parameter("control_rate").value
        self._timer = self.create_timer(1.0 / rate, self._control_cb)
        self._last_tick = time.monotonic()

        self.get_logger().info(
            f"FaceTrackServoNode ready "
            f"(rate={rate}Hz, fov={self._fov_h}×{self._fov_v}°, "
            f"pan±{self._pan_limit}°, tilt±{self._tilt_limit}°)"
        )

    def _reload_params(self) -> None:
        self._fov_h       = self.get_parameter("fov_h_deg").value
        self._fov_v       = self.get_parameter("fov_v_deg").value
        self._pan_limit   = self.get_parameter("pan_limit_deg").value
        self._tilt_limit  = self.get_parameter("tilt_limit_deg").value
        self._dead_zone   = self.get_parameter("dead_zone").value
        self._lost_t      = self.get_parameter("lost_timeout_s").value
        self._return_rate = self.get_parameter("return_rate_deg_s").value

        self._pid_pan  = PIDController(
            kp=self.get_parameter("kp_pan").value,
            ki=self.get_parameter("ki_pan").value,
            kd=self.get_parameter("kd_pan").value,
            vel_limit=self.get_parameter("pan_vel_limit").value,
            windup_limit=self.get_parameter("windup_limit").value,
        )
        self._pid_tilt = PIDController(
            kp=self.get_parameter("kp_tilt").value,
            ki=self.get_parameter("ki_tilt").value,
            kd=self.get_parameter("kd_tilt").value,
            vel_limit=self.get_parameter("tilt_vel_limit").value,
            windup_limit=self.get_parameter("windup_limit").value,
        )

    # ── Subscription callback ──────────────────────────────────────────────

    def _on_faces(self, msg) -> None:
        face = pick_closest_face(msg.faces)
        with self._lock:
            self._latest_face  = face
            if face is not None:
                self._last_face_t = time.monotonic()

    # ── Control loop ───────────────────────────────────────────────────────

    def _control_cb(self) -> None:
        now = time.monotonic()
        dt  = now - self._last_tick
        self._last_tick = now
        dt = max(dt, 1e-4)  # guard against zero dt at startup

        with self._lock:
            face       = self._latest_face
            last_face_t = self._last_face_t

        face_fresh = (last_face_t > 0.0 and (now - last_face_t) < self._lost_t)

        if not face_fresh or face is None:
            # Return to centre
            self._pid_pan.reset()
            self._pid_tilt.reset()
            step = self._return_rate * dt
            self._pan_cmd  = _step_toward_zero(self._pan_cmd,  step)
            self._tilt_cmd = _step_toward_zero(self._tilt_cmd, step)
        else:
            pan_err, tilt_err = face_image_error(face, self._fov_h, self._fov_v)

            # Dead zone (normalised fraction → degrees)
            dead_deg_h = self._dead_zone * self._fov_h
            dead_deg_v = self._dead_zone * self._fov_v

            if abs(pan_err) < dead_deg_h:
                self._pid_pan.reset()
            else:
                vel_pan = self._pid_pan.update(pan_err, dt)
                self._pan_cmd = clamp(
                    self._pan_cmd + vel_pan * dt,
                    -self._pan_limit, self._pan_limit,
                )

            if abs(tilt_err) < dead_deg_v:
                self._pid_tilt.reset()
            else:
                vel_tilt = self._pid_tilt.update(tilt_err, dt)
                self._tilt_cmd = clamp(
                    self._tilt_cmd + vel_tilt * dt,
                    -self._tilt_limit, self._tilt_limit,
                )

        pan_msg  = Float32(); pan_msg.data  = float(self._pan_cmd)
        tilt_msg = Float32(); tilt_msg.data = float(self._tilt_cmd)
        self._pan_pub.publish(pan_msg)
        self._tilt_pub.publish(tilt_msg)


def _step_toward_zero(value: float, step: float) -> float:
    """Move value toward 0 by step without overshooting."""
    if abs(value) <= step:
        return 0.0
    return value - math.copysign(step, value)


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