"""
Follow Me ROS2 node.

Runs person tracking (RealSense + MediaPipe) and follower controller.
Publishes geometry_msgs/Twist to cmd_vel and advertises Follow Me services
for the AgenticROS plugin.
"""

from __future__ import annotations

import json

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist as TwistMsg
from std_msgs.msg import String as StringMsg
from agenticros_msgs.srv import (
    FollowMeStart,
    FollowMeStop,
    FollowMeSetDistance,
    FollowMeGetStatus,
    FollowMeSetTarget,
)

from .person_tracker import PersonTracker
from .follower_controller import FollowerController, ControllerConfig


def _twist_to_msg(linear_x: float, angular_z: float) -> TwistMsg:
    msg = TwistMsg()
    msg.linear.x = float(linear_x)
    msg.linear.y = 0.0
    msg.linear.z = 0.0
    msg.angular.x = 0.0
    msg.angular.y = 0.0
    msg.angular.z = float(angular_z)
    return msg


class FollowMeNode(Node):
    """ROS2 node: person tracking, follower control, cmd_vel publishing, Follow Me services."""

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

        self.declare_parameter("use_camera", True)
        self.declare_parameter("target_distance", 1.0)
        self.declare_parameter("cmd_vel_topic", "cmd_vel")

        use_camera = self.get_parameter("use_camera").value
        target_distance = float(self.get_parameter("target_distance").value)
        cmd_vel_topic = self.get_parameter("cmd_vel_topic").value

        self.tracker = PersonTracker(use_camera=use_camera)
        config = ControllerConfig(target_distance=target_distance)
        self.controller = FollowerController(config=config)

        self._cmd_vel_pub = self.create_publisher(TwistMsg, cmd_vel_topic, 10)
        self._control_timer = self.create_timer(1.0 / 30.0, self._control_callback)

        self.create_service(FollowMeStart, "follow_me/start", self._handle_start)
        self.create_service(FollowMeStop, "follow_me/stop", self._handle_stop)
        self.create_service(
            FollowMeSetDistance, "follow_me/set_distance", self._handle_set_distance
        )
        self.create_service(
            FollowMeGetStatus, "follow_me/get_status", self._handle_get_status
        )
        self.create_service(
            FollowMeSetTarget, "follow_me/set_target", self._handle_set_target
        )

        # Topic-based command/status interface for transports without service support (e.g. Zenoh).
        self.create_subscription(StringMsg, "follow_me/cmd", self._handle_cmd_msg, 10)
        self._status_pub = self.create_publisher(StringMsg, "follow_me/status", 10)
        self._status_timer = self.create_timer(0.5, self._publish_status)

        self.tracker.start()
        self.get_logger().info(
            f"Follow Me node started: cmd_vel={cmd_vel_topic}, "
            f"target_distance={target_distance}m, use_camera={use_camera}"
        )

    def _get_target_person(self):
        """Resolve target person from controller target_person_id or closest."""
        persons = self.tracker.persons
        if not persons:
            return None
        tid = self.controller.target_person_id
        if tid is not None:
            for p in persons:
                if p.id == tid:
                    return p
        return min(persons, key=lambda p: p.distance)

    def _control_callback(self) -> None:
        """Run at 30 Hz: update controller and publish cmd_vel."""
        target = self._get_target_person()
        twist = self.controller.update(target)
        msg = _twist_to_msg(twist.linear_x, twist.angular_z)
        self._cmd_vel_pub.publish(msg)

    def _handle_start(
        self, request: FollowMeStart.Request, response: FollowMeStart.Response
    ) -> FollowMeStart.Response:
        desc = request.target_description.strip() if request.target_description else ""
        if desc:
            self.controller.start(target_description=desc)
            response.success = True
            response.message = f"Started following: {desc}"
        else:
            self.controller.start()
            response.success = True
            response.message = "Started following closest person"
        return response

    def _handle_stop(
        self, request: FollowMeStop.Request, response: FollowMeStop.Response
    ) -> FollowMeStop.Response:
        self.controller.stop()
        response.success = True
        response.message = "Stopped following"
        return response

    def _handle_set_distance(
        self,
        request: FollowMeSetDistance.Request,
        response: FollowMeSetDistance.Response,
    ) -> FollowMeSetDistance.Response:
        d = float(request.distance)
        if d < 0.2 or d > 5.0:
            response.success = False
            response.target_distance = self.controller.config.target_distance
            return response
        self.controller.set_target_distance(d)
        response.success = True
        response.target_distance = self.controller.config.target_distance
        return response

    def _handle_get_status(
        self,
        request: FollowMeGetStatus.Request,
        response: FollowMeGetStatus.Response,
    ) -> FollowMeGetStatus.Response:
        response.success = True
        response.enabled = self.controller.enabled
        response.tracking = (
            self.controller.mode.value == "follow" and self._get_target_person() is not None
        )
        response.target_distance = self.controller.config.target_distance
        response.target_person_id = int(self.controller.target_person_id or 0)
        response.target_description = self.controller.target_description or ""
        response.persons_detected = len(self.tracker.persons)
        target = self._get_target_person()
        response.current_distance = float(target.z) if target else 0.0
        t = self.controller._last_twist
        response.twist.linear.x = t.linear_x
        response.twist.angular.z = t.angular_z
        response.error_message = ""
        return response

    def _handle_set_target(
        self,
        request: FollowMeSetTarget.Request,
        response: FollowMeSetTarget.Response,
    ) -> FollowMeSetTarget.Response:
        desc = (request.description or "").strip()
        if not desc:
            response.success = False
            response.message = "Missing description"
            return response
        persons = self.tracker.persons
        if not persons:
            response.success = False
            response.person_id = 0
            response.confidence = 0.0
            response.message = "No persons detected"
            return response
        # Without VLM we lock to closest person and store description
        closest = min(persons, key=lambda p: p.distance)
        self.controller.set_target_person(closest.id)
        self.controller.target_description = desc
        response.success = True
        response.person_id = int(closest.id)
        response.confidence = float(closest.confidence)
        response.message = f"Locked onto person #{closest.id} (closest match)"
        return response

    def _handle_cmd_msg(self, msg: StringMsg) -> None:
        """Dispatch JSON commands from follow_me/cmd. Mirrors the service handlers."""
        text = (msg.data or "").strip()
        if not text:
            return
        try:
            cmd = json.loads(text)
        except (TypeError, ValueError):
            self.get_logger().warn(f"follow_me/cmd: invalid JSON: {text!r}")
            return
        if not isinstance(cmd, dict):
            self.get_logger().warn("follow_me/cmd: payload must be a JSON object")
            return
        action = str(cmd.get("action") or "").strip().lower()
        if action == "start":
            desc = str(cmd.get("target") or cmd.get("target_description") or "").strip()
            if desc:
                self.controller.start(target_description=desc)
            else:
                self.controller.start()
            self.get_logger().info(f"follow_me/cmd: start{' (' + desc + ')' if desc else ' (closest)'}")
        elif action == "stop":
            self.controller.stop()
            self.get_logger().info("follow_me/cmd: stop")
        elif action == "set_distance":
            try:
                d = float(cmd.get("distance"))
            except (TypeError, ValueError):
                self.get_logger().warn("follow_me/cmd set_distance: invalid distance")
                return
            if 0.2 <= d <= 5.0:
                self.controller.set_target_distance(d)
                self.get_logger().info(f"follow_me/cmd: set_distance={d}")
            else:
                self.get_logger().warn(f"follow_me/cmd set_distance out of range: {d}")
        elif action == "set_target":
            desc = str(cmd.get("description") or cmd.get("target") or "").strip()
            if not desc:
                self.get_logger().warn("follow_me/cmd set_target: missing description")
                return
            persons = self.tracker.persons
            if not persons:
                self.get_logger().warn("follow_me/cmd set_target: no persons detected")
                return
            closest = min(persons, key=lambda p: p.distance)
            self.controller.set_target_person(closest.id)
            self.controller.target_description = desc
            self.get_logger().info(f"follow_me/cmd: set_target={desc} -> person #{closest.id}")
        else:
            self.get_logger().warn(f"follow_me/cmd: unknown action {action!r}")

    def _publish_status(self) -> None:
        """Publish current follow-me state as JSON on follow_me/status (2 Hz)."""
        target = self._get_target_person()
        t = self.controller._last_twist
        status = {
            "enabled": bool(self.controller.enabled),
            "tracking": bool(
                self.controller.mode.value == "follow" and target is not None
            ),
            "target_distance": float(self.controller.config.target_distance),
            "target_person_id": int(self.controller.target_person_id or 0),
            "target_description": str(self.controller.target_description or ""),
            "persons_detected": int(len(self.tracker.persons)),
            "current_distance": float(target.z) if target else 0.0,
            "twist": {
                "linear_x": float(t.linear_x),
                "angular_z": float(t.angular_z),
            },
        }
        out = StringMsg()
        out.data = json.dumps(status)
        self._status_pub.publish(out)


def main(args=None):
    rclpy.init(args=args)
    node = FollowMeNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.tracker.stop()
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
