"""
person_follower.launch.py — Launch the proportional person-following controller.

The person_follower_node subscribes to /person/target (PoseStamped in base_link
frame) published by sl-jetson's person detector and drives /cmd_vel accordingly.

PREREQUISITES (must be running before this launch):
  1. cmd_vel_bridge_node (saltybot_bridge) — serial port owner, safety wrapping
  2. sl-jetson's person_detector node       — publishes /person/target
  3. Nav2 local costmap                     — publishes /local_costmap/costmap
     (optional: obstacle avoidance is skipped if no costmap messages arrive)

Usage:
  # Defaults (1.5m follow distance, 0.5m/s max, 2s lost timeout):
  ros2 launch saltybot_follower person_follower.launch.py

  # Closer following for indoor tight spaces:
  ros2 launch saltybot_follower person_follower.launch.py follow_distance:=1.0

  # Disable obstacle avoidance (for environments without Nav2 costmap):
  ros2 launch saltybot_follower person_follower.launch.py obstacle_threshold:=100

  # Load all params from YAML:
  ros2 launch saltybot_follower person_follower.launch.py \\
      params_file:=/path/to/person_follower_params.yaml

Safety note:
  To disable at runtime without stopping the node:
    ros2 param set /person_follower follow_enabled false
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node


def _launch_follower(context, *args, **kwargs):
    params_file = LaunchConfiguration("params_file").perform(context)

    inline_params = {
        "follow_distance":     float(LaunchConfiguration("follow_distance").perform(context)),
        "dead_zone":           float(LaunchConfiguration("dead_zone").perform(context)),
        "kp_linear":           float(LaunchConfiguration("kp_linear").perform(context)),
        "kp_angular":          float(LaunchConfiguration("kp_angular").perform(context)),
        "max_linear_vel":      float(LaunchConfiguration("max_linear_vel").perform(context)),
        "max_angular_vel":     float(LaunchConfiguration("max_angular_vel").perform(context)),
        "lost_timeout":        float(LaunchConfiguration("lost_timeout").perform(context)),
        "search_timeout":      float(LaunchConfiguration("search_timeout").perform(context)),
        "search_angular_vel":  float(LaunchConfiguration("search_angular_vel").perform(context)),
        "obstacle_check_dist": float(LaunchConfiguration("obstacle_check_dist").perform(context)),
        "obstacle_threshold":  int(LaunchConfiguration("obstacle_threshold").perform(context)),
        "robot_width":         float(LaunchConfiguration("robot_width").perform(context)),
        "control_rate":        float(LaunchConfiguration("control_rate").perform(context)),
        "follow_enabled":      LaunchConfiguration("follow_enabled").perform(context).lower() == "true",
    }

    node_params = [params_file, inline_params] if params_file else [inline_params]

    return [Node(
        package="saltybot_follower",
        executable="person_follower_node",
        name="person_follower",
        output="screen",
        parameters=node_params,
    )]


def generate_launch_description():
    pkg_share = get_package_share_directory("saltybot_follower")
    default_params = os.path.join(pkg_share, "config", "person_follower_params.yaml")

    return LaunchDescription([
        DeclareLaunchArgument(
            "params_file",
            default_value=default_params,
            description="Full path to person_follower_params.yaml (overrides inline args)"),

        # Follow geometry
        DeclareLaunchArgument(
            "follow_distance",  default_value="1.5",
            description="Target follow distance (m)"),
        DeclareLaunchArgument(
            "dead_zone",        default_value="0.3",
            description="Dead zone around follow_distance — no linear cmd (m)"),

        # Gains
        DeclareLaunchArgument(
            "kp_linear",        default_value="0.6",
            description="Proportional gain: distance error → linear.x"),
        DeclareLaunchArgument(
            "kp_angular",       default_value="1.2",
            description="Proportional gain: bearing error → angular.z"),

        # Velocity limits
        DeclareLaunchArgument(
            "max_linear_vel",   default_value="0.5",
            description="Max forward/backward speed (m/s)"),
        DeclareLaunchArgument(
            "max_angular_vel",  default_value="1.0",
            description="Max turn rate (rad/s)"),

        # Lost-target timeouts
        DeclareLaunchArgument(
            "lost_timeout",     default_value="2.0",
            description="Seconds after last /person/target before stopping"),
        DeclareLaunchArgument(
            "search_timeout",   default_value="5.0",
            description="Seconds after last /person/target before slow rotation search"),
        DeclareLaunchArgument(
            "search_angular_vel", default_value="0.3",
            description="Rotation speed while searching for person (rad/s)"),

        # Obstacle avoidance
        DeclareLaunchArgument(
            "obstacle_check_dist", default_value="0.5",
            description="Forward look-ahead distance for obstacle check (m)"),
        DeclareLaunchArgument(
            "obstacle_threshold",  default_value="70",
            description="Costmap cell value treated as obstacle (Nav2 lethal=99)"),
        DeclareLaunchArgument(
            "robot_width",         default_value="0.3",
            description="Robot body width for obstacle corridor check (m)"),

        # Control
        DeclareLaunchArgument(
            "control_rate",     default_value="20.0",
            description="Control loop frequency (Hz)"),
        DeclareLaunchArgument(
            "follow_enabled",   default_value="true",
            description="Master enable; set false to pause following without stopping node"),

        OpaqueFunction(function=_launch_follower),
    ])
