#!/usr/bin/env python3
"""Real-robot bringup for the custom Hybrid A* and Pure Pursuit stack.

This launch intentionally does not start Nav2 controller_server. The custom
planner publishes /plan and the tracker publishes an internal Twist command
that is converted to /ackermann_cmd by planner_ackermann_cmd_bridge.
"""

import os

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    planner_share = get_package_share_directory("planner")
    base_share = get_package_share_directory("origincar_base")
    description_share = get_package_share_directory("origincar_description")

    map_yaml = LaunchConfiguration("map")
    lidar_params = LaunchConfiguration("lidar_params")
    lidar_serial_port = LaunchConfiguration("lidar_serial_port")
    lidar_model = LaunchConfiguration("lidar_model")
    start_lidar = LaunchConfiguration("start_lidar")
    serial_port = LaunchConfiguration("serial_port")
    serial_baud = LaunchConfiguration("serial_baud")
    enable_motion = LaunchConfiguration("enable_motion")
    wheelbase = LaunchConfiguration("wheelbase")
    max_steering_angle = LaunchConfiguration("max_steering_angle")
    lookahead_distance = LaunchConfiguration("lookahead_distance")
    min_lookahead_distance = LaunchConfiguration("min_lookahead_distance")
    lookahead_curvature_gain = LaunchConfiguration("lookahead_curvature_gain")
    lookahead_error_gain = LaunchConfiguration("lookahead_error_gain")
    heading_correction_gain = LaunchConfiguration("heading_correction_gain")
    cross_track_correction_gain = LaunchConfiguration("cross_track_correction_gain")
    cross_track_speed_gain = LaunchConfiguration("cross_track_speed_gain")
    min_tracking_speed_ratio = LaunchConfiguration("min_tracking_speed_ratio")
    path_reacquire_distance = LaunchConfiguration("path_reacquire_distance")
    linear_speed = LaunchConfiguration("linear_speed")
    min_linear_speed = LaunchConfiguration("min_linear_speed")
    reverse_speed = LaunchConfiguration("reverse_speed")
    slowdown_distance = LaunchConfiguration("slowdown_distance")
    curvature_slowdown_gain = LaunchConfiguration("curvature_slowdown_gain")
    min_curvature_speed_ratio = LaunchConfiguration("min_curvature_speed_ratio")
    min_goal_slowdown_ratio = LaunchConfiguration("min_goal_slowdown_ratio")
    max_angular_speed = LaunchConfiguration("max_angular_speed")
    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
    heuristic_weight = LaunchConfiguration("heuristic_weight")
    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
    state_log_period = LaunchConfiguration("state_log_period")
    search_progress_log_period_ms = LaunchConfiguration("search_progress_log_period_ms")
    use_sim_time = LaunchConfiguration("use_sim_time")
    serial_baud_value = ParameterValue(serial_baud, value_type=int)
    enable_motion_value = ParameterValue(enable_motion, value_type=bool)
    wheelbase_value = ParameterValue(wheelbase, value_type=float)
    max_steering_angle_value = ParameterValue(max_steering_angle, value_type=float)
    lookahead_distance_value = ParameterValue(lookahead_distance, value_type=float)
    min_lookahead_distance_value = ParameterValue(
        min_lookahead_distance, value_type=float
    )
    lookahead_curvature_gain_value = ParameterValue(
        lookahead_curvature_gain, value_type=float
    )
    lookahead_error_gain_value = ParameterValue(
        lookahead_error_gain, value_type=float
    )
    heading_correction_gain_value = ParameterValue(
        heading_correction_gain, value_type=float
    )
    cross_track_correction_gain_value = ParameterValue(
        cross_track_correction_gain, value_type=float
    )
    cross_track_speed_gain_value = ParameterValue(
        cross_track_speed_gain, value_type=float
    )
    min_tracking_speed_ratio_value = ParameterValue(
        min_tracking_speed_ratio, value_type=float
    )
    path_reacquire_distance_value = ParameterValue(
        path_reacquire_distance, value_type=float
    )
    linear_speed_value = ParameterValue(linear_speed, value_type=float)
    min_linear_speed_value = ParameterValue(min_linear_speed, value_type=float)
    reverse_speed_value = ParameterValue(reverse_speed, value_type=float)
    slowdown_distance_value = ParameterValue(slowdown_distance, value_type=float)
    curvature_slowdown_gain_value = ParameterValue(
        curvature_slowdown_gain, value_type=float
    )
    min_curvature_speed_ratio_value = ParameterValue(
        min_curvature_speed_ratio, value_type=float
    )
    min_goal_slowdown_ratio_value = ParameterValue(
        min_goal_slowdown_ratio, value_type=float
    )
    max_angular_speed_value = ParameterValue(max_angular_speed, value_type=float)
    use_grid_heuristic_value = ParameterValue(use_grid_heuristic, value_type=bool)
    heuristic_weight_value = ParameterValue(heuristic_weight, value_type=float)
    goal_yaw_heuristic_weight_value = ParameterValue(
        goal_yaw_heuristic_weight, value_type=float
    )
    snap_final_xy_to_goal_value = ParameterValue(
        snap_final_xy_to_goal, value_type=bool
    )
    state_log_period_value = ParameterValue(state_log_period, value_type=float)
    search_progress_log_period_ms_value = ParameterValue(
        search_progress_log_period_ms, value_type=float
    )
    use_sim_time_value = ParameterValue(use_sim_time, value_type=bool)

    urdf_path = os.path.join(description_share, "urdf", "origincar.urdf")
    ekf_path = os.path.join(base_share, "config", "ekf.yaml")
    amcl_path = os.path.join(planner_share, "config", "real_amcl.yaml")
    planner_path = os.path.join(planner_share, "config", "real_hybrid_astar.yaml")
    tracker_path = os.path.join(planner_share, "config", "real_pure_pursuit.yaml")

    # xacro is not needed here because origincar.urdf is already generated and
    # is the file used by the existing real-robot launch files.
    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        parameters=[{"robot_description": open(urdf_path, encoding="utf-8").read(),
                     "use_sim_time": use_sim_time_value}],
    )

    base_node = Node(
        package="origincar_base",
        executable="origincar_base_node",
        name="origincar_base",
        output="screen",
        parameters=[{
            "usart_port_name": serial_port,
            "serial_baud_rate": serial_baud_value,
            "robot_frame_id": "base_link",
            "odom_frame_id": "odom",
            "gyro_frame_id": "gyro_link",
            # The bridge consumes /planner_cmd_vel. This prevents the same
            # command from reaching the serial driver in two formats.
            "cmd_vel": "/planner_disabled_cmd_vel",
            "akm_cmd_vel": "/ackermann_cmd",
            "publish_tf": False,
            "gyro_z_low_pass_alpha": 0.6,
        }],
    )

    cmd_vel_bridge = Node(
        package="planner",
        executable="ackermann_cmd_bridge.py",
        name="planner_ackermann_cmd_bridge",
        output="screen",
        condition=IfCondition(enable_motion),
        parameters=[{
            "input_topic": "/planner_cmd_vel",
            "output_topic": "/ackermann_cmd",
            "wheelbase": wheelbase_value,
            "frame_id": "base_link",
            "max_steering_angle": max_steering_angle_value,
            "command_timeout": 0.25,
            "watchdog_rate": 10.0,
            "state_log_period": state_log_period_value,
        }],
    )

    ekf_node = Node(
        package="robot_localization",
        executable="ekf_node",
        name="ekf_filter_node",
        output="screen",
        parameters=[ekf_path, {"use_sim_time": use_sim_time_value}],
        remappings=[("odometry/filtered", "/odom_combined")],
    )

    lidar_node = Node(
        package="lslidar_driver",
        executable="lslidar_driver_node",
        name="lslidar_driver_node",
        output="screen",
        condition=IfCondition(start_lidar),
        parameters=[lidar_params, {
            "interface_selection": "serial",
            "serial_port_": lidar_serial_port,
            "lidar_name": lidar_model,
        }],
    )

    map_server = Node(
        package="nav2_map_server",
        executable="map_server",
        name="map_server",
        output="screen",
        parameters=[{"yaml_filename": map_yaml, "use_sim_time": use_sim_time_value}],
    )

    amcl_node = Node(
        package="nav2_amcl",
        executable="amcl",
        name="amcl",
        output="screen",
        parameters=[amcl_path, {"use_sim_time": use_sim_time_value}],
    )

    lifecycle_manager = Node(
        package="nav2_lifecycle_manager",
        executable="lifecycle_manager",
        name="lifecycle_manager_localization",
        output="screen",
        parameters=[{
            "use_sim_time": use_sim_time_value,
            "autostart": True,
            "node_names": ["map_server", "amcl"],
        }],
    )

    planner_node = Node(
        package="planner",
        executable="grid_astar_theta_node",
        name="grid_astar_theta_planner",
        output="screen",
        parameters=[planner_path, {
            "use_sim_time": use_sim_time_value,
            "use_grid_heuristic": use_grid_heuristic_value,
            "heuristic_weight": heuristic_weight_value,
            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
            "state_log_period": state_log_period_value,
            "search_progress_log_period_ms": search_progress_log_period_ms_value,
        }],
    )

    tracker_node = Node(
        package="planner",
        executable="topology_pure_pursuit_node.py",
        name="topology_pure_pursuit",
        output="screen",
        parameters=[tracker_path, {
            "use_sim_time": use_sim_time_value,
            "publish_cmd_vel": enable_motion_value,
            "cmd_vel_topic": "/planner_cmd_vel",
            "lookahead_distance": lookahead_distance_value,
            "min_lookahead_distance": min_lookahead_distance_value,
            "lookahead_curvature_gain": lookahead_curvature_gain_value,
            "lookahead_error_gain": lookahead_error_gain_value,
            "heading_correction_gain": heading_correction_gain_value,
            "cross_track_correction_gain": cross_track_correction_gain_value,
            "cross_track_speed_gain": cross_track_speed_gain_value,
            "min_tracking_speed_ratio": min_tracking_speed_ratio_value,
            "path_reacquire_distance": path_reacquire_distance_value,
            "linear_speed": linear_speed_value,
            "min_linear_speed": min_linear_speed_value,
            "reverse_speed": reverse_speed_value,
            "slowdown_distance": slowdown_distance_value,
            "curvature_slowdown_gain": curvature_slowdown_gain_value,
            "min_curvature_speed_ratio": min_curvature_speed_ratio_value,
            "min_goal_slowdown_ratio": min_goal_slowdown_ratio_value,
            "max_angular_speed": max_angular_speed_value,
            "state_log_period": state_log_period_value,
        }],
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            "map",
            description="Required: real 5x5m map YAML used by map_server.",
        ),
        DeclareLaunchArgument(
            "lidar_params",
            description="Required: LSLIDAR YAML matching the actual sensor/interface.",
        ),
        DeclareLaunchArgument("lidar_serial_port", default_value="/dev/radar"),
        DeclareLaunchArgument("lidar_model", default_value="N10"),
        DeclareLaunchArgument(
            "start_lidar",
            default_value="true",
            description="Start LSLIDAR. Set false when /scan is provided externally.",
        ),
        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
        DeclareLaunchArgument("serial_baud", default_value="921600"),
        DeclareLaunchArgument(
            "enable_motion",
            default_value="false",
            description="Publish motion commands. Keep false for the first planner-only test.",
        ),
        DeclareLaunchArgument("wheelbase", default_value="0.143"),
        DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
        DeclareLaunchArgument("lookahead_distance", default_value="0.30"),
        DeclareLaunchArgument("min_lookahead_distance", default_value="0.12"),
        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.70"),
        DeclareLaunchArgument("lookahead_error_gain", default_value="1.60"),
        DeclareLaunchArgument("heading_correction_gain", default_value="0.70"),
        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.00"),
        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.50"),
        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
        DeclareLaunchArgument("linear_speed", default_value="0.18"),
        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
        DeclareLaunchArgument("reverse_speed", default_value="0.08"),
        DeclareLaunchArgument("slowdown_distance", default_value="0.70"),
        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.12"),
        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.65"),
        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
        DeclareLaunchArgument("max_angular_speed", default_value="1.50"),
        DeclareLaunchArgument(
            "use_grid_heuristic",
            default_value="true",
            description="Use obstacle-aware 2D grid distance as the Hybrid A* heuristic.",
        ),
        DeclareLaunchArgument(
            "heuristic_weight",
            default_value="1.25",
            description="Weighted A* heuristic multiplier. Higher is faster but less optimal.",
        ),
        DeclareLaunchArgument(
            "goal_yaw_heuristic_weight",
            default_value="0.0",
            description="Weight for final yaw guidance in the Hybrid A* heuristic.",
        ),
        DeclareLaunchArgument(
            "snap_final_xy_to_goal",
            default_value="true",
            description="Publish the final path XY exactly at the requested goal XY without forcing yaw.",
        ),
        DeclareLaunchArgument(
            "state_log_period",
            default_value="1.0",
            description="Seconds between runtime state logs. Set 0.0 to disable.",
        ),
        DeclareLaunchArgument(
            "search_progress_log_period_ms",
            default_value="500.0",
            description="Milliseconds between Hybrid A* search progress logs. Set 0.0 to disable.",
        ),
        DeclareLaunchArgument("use_sim_time", default_value="false"),
        robot_state_publisher,
        base_node,
        cmd_vel_bridge,
        ekf_node,
        lidar_node,
        map_server,
        amcl_node,
        lifecycle_manager,
        planner_node,
        tracker_node,
    ])
