#!/usr/bin/env python3
"""Odom-only bringup for Hybrid A* planning and Pure Pursuit tracking.

This launch does not start lidar or AMCL. The user provides the known current
map pose; planner_odom_map_tf aligns map->odom once, then localization is
wheel odom only.
"""

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")
    description_share = get_package_share_directory("origincar_description")

    map_yaml = LaunchConfiguration("map")
    use_map = LaunchConfiguration("use_map")
    start_base = LaunchConfiguration("start_base")
    start_robot_state_publisher = LaunchConfiguration("start_robot_state_publisher")
    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")
    map_frame = LaunchConfiguration("map_frame")
    odom_frame = LaunchConfiguration("odom_frame")
    base_frame = LaunchConfiguration("base_frame")
    odom_topic = LaunchConfiguration("odom_topic")
    initial_x = LaunchConfiguration("initial_x")
    initial_y = LaunchConfiguration("initial_y")
    initial_yaw = LaunchConfiguration("initial_yaw")
    initialize_on_first_odom = LaunchConfiguration("initialize_on_first_odom")
    planner_min_turning_radius = LaunchConfiguration("planner_min_turning_radius")
    tracker_min_turning_radius = LaunchConfiguration("tracker_min_turning_radius")
    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")
    goal_tolerance = LaunchConfiguration("goal_tolerance")
    goal_yaw_tolerance = LaunchConfiguration("goal_yaw_tolerance")
    align_goal_yaw = LaunchConfiguration("align_goal_yaw")
    final_alignment_offset = LaunchConfiguration("final_alignment_offset")
    final_alignment_speed = LaunchConfiguration("final_alignment_speed")
    require_goal_yaw = LaunchConfiguration("require_goal_yaw")
    planner_goal_xy_tolerance = LaunchConfiguration("planner_goal_xy_tolerance")
    snap_final_xy_to_goal = LaunchConfiguration("snap_final_xy_to_goal")
    use_grid_heuristic = LaunchConfiguration("use_grid_heuristic")
    heuristic_weight = LaunchConfiguration("heuristic_weight")
    goal_yaw_heuristic_weight = LaunchConfiguration("goal_yaw_heuristic_weight")
    planner_resolution = LaunchConfiguration("planner_resolution")
    yaw_bins = LaunchConfiguration("yaw_bins")
    primitive_length = LaunchConfiguration("primitive_length")
    max_iterations = LaunchConfiguration("max_iterations")
    planning_timeout_ms = LaunchConfiguration("planning_timeout_ms")
    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)
    initial_x_value = ParameterValue(initial_x, value_type=float)
    initial_y_value = ParameterValue(initial_y, value_type=float)
    initial_yaw_value = ParameterValue(initial_yaw, value_type=float)
    initialize_on_first_odom_value = ParameterValue(
        initialize_on_first_odom, value_type=bool
    )
    planner_min_turning_radius_value = ParameterValue(
        planner_min_turning_radius, value_type=float
    )
    tracker_min_turning_radius_value = ParameterValue(
        tracker_min_turning_radius, 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)
    goal_tolerance_value = ParameterValue(goal_tolerance, value_type=float)
    goal_yaw_tolerance_value = ParameterValue(goal_yaw_tolerance, value_type=float)
    align_goal_yaw_value = ParameterValue(align_goal_yaw, value_type=bool)
    final_alignment_offset_value = ParameterValue(
        final_alignment_offset, value_type=float
    )
    final_alignment_speed_value = ParameterValue(
        final_alignment_speed, value_type=float
    )
    require_goal_yaw_value = ParameterValue(require_goal_yaw, value_type=bool)
    planner_goal_xy_tolerance_value = ParameterValue(
        planner_goal_xy_tolerance, value_type=float
    )
    snap_final_xy_to_goal_value = ParameterValue(
        snap_final_xy_to_goal, value_type=bool
    )
    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
    )
    planner_resolution_value = ParameterValue(planner_resolution, value_type=float)
    yaw_bins_value = ParameterValue(yaw_bins, value_type=int)
    primitive_length_value = ParameterValue(primitive_length, value_type=float)
    max_iterations_value = ParameterValue(max_iterations, value_type=int)
    planning_timeout_ms_value = ParameterValue(planning_timeout_ms, value_type=float)
    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")
    planner_path = os.path.join(planner_share, "config", "odom_hybrid_astar.yaml")
    tracker_path = os.path.join(planner_share, "config", "odom_pure_pursuit.yaml")

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="screen",
        condition=IfCondition(start_robot_state_publisher),
        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",
        condition=IfCondition(start_base),
        parameters=[{
            "usart_port_name": serial_port,
            "serial_baud_rate": serial_baud_value,
            "robot_frame_id": base_frame,
            "odom_frame_id": odom_frame,
            "gyro_frame_id": "gyro_link",
            "cmd_vel": "/planner_disabled_cmd_vel",
            "akm_cmd_vel": "/ackermann_cmd",
            "publish_tf": True,
            "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_frame,
            "max_steering_angle": max_steering_angle_value,
            "command_timeout": 0.25,
            "watchdog_rate": 10.0,
            "state_log_period": state_log_period_value,
        }],
    )

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

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

    odom_map_tf = Node(
        package="planner",
        executable="odom_map_tf.py",
        name="planner_odom_map_tf",
        output="screen",
        parameters=[{
            "use_sim_time": use_sim_time_value,
            "map_frame": map_frame,
            "odom_frame": odom_frame,
            "odom_topic": odom_topic,
            "initial_x": initial_x_value,
            "initial_y": initial_y_value,
            "initial_yaw": initial_yaw_value,
            "initialize_on_first_odom": initialize_on_first_odom_value,
            "state_log_period": state_log_period_value,
        }],
    )

    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,
            "map_frame": map_frame,
            "odom_topic": odom_topic,
            "min_turning_radius": planner_min_turning_radius_value,
            "require_goal_yaw": require_goal_yaw_value,
            "goal_xy_tolerance": planner_goal_xy_tolerance_value,
            "snap_final_xy_to_goal": snap_final_xy_to_goal_value,
            "use_grid_heuristic": use_grid_heuristic_value,
            "heuristic_weight": heuristic_weight_value,
            "goal_yaw_heuristic_weight": goal_yaw_heuristic_weight_value,
            "resolution": planner_resolution_value,
            "yaw_bins": yaw_bins_value,
            "primitive_length": primitive_length_value,
            "max_iterations": max_iterations_value,
            "planning_timeout_ms": planning_timeout_ms_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,
            "map_frame": map_frame,
            "odom_topic": odom_topic,
            "publish_cmd_vel": enable_motion_value,
            "cmd_vel_topic": "/planner_cmd_vel",
            "min_turning_radius": tracker_min_turning_radius_value,
            "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,
            "goal_tolerance": goal_tolerance_value,
            "goal_yaw_tolerance": goal_yaw_tolerance_value,
            "align_goal_yaw": align_goal_yaw_value,
            "final_alignment_offset": final_alignment_offset_value,
            "final_alignment_speed": final_alignment_speed_value,
            "state_log_period": state_log_period_value,
        }],
    )

    return LaunchDescription([
        DeclareLaunchArgument(
            "map",
            default_value=os.path.join(planner_share, "maps", "real_10x10.yaml"),
            description="Static map YAML. Used only when use_map=true.",
        ),
        DeclareLaunchArgument("use_map", default_value="true"),
        DeclareLaunchArgument("start_base", default_value="true"),
        DeclareLaunchArgument("start_robot_state_publisher", default_value="true"),
        DeclareLaunchArgument("serial_port", default_value="/dev/ttyACM0"),
        DeclareLaunchArgument("serial_baud", default_value="115200"),
        DeclareLaunchArgument(
            "enable_motion",
            default_value="false",
            description="Publish motion commands. Keep false for planner-only tests.",
        ),
        DeclareLaunchArgument("wheelbase", default_value="0.143"),
        DeclareLaunchArgument("max_steering_angle", default_value="0.60"),
        DeclareLaunchArgument("map_frame", default_value="map"),
        DeclareLaunchArgument("odom_frame", default_value="odom"),
        DeclareLaunchArgument("base_frame", default_value="base_link"),
        DeclareLaunchArgument("odom_topic", default_value="/odom"),
        DeclareLaunchArgument("initial_x", default_value="0.0"),
        DeclareLaunchArgument("initial_y", default_value="0.0"),
        DeclareLaunchArgument("initial_yaw", default_value="0.0"),
        DeclareLaunchArgument(
            "initialize_on_first_odom",
            default_value="true",
            description="Use launch initial pose when the first odom message arrives.",
        ),
        DeclareLaunchArgument(
            "planner_min_turning_radius",
            default_value="0.25",
            description="Hybrid A* minimum turning radius in meters.",
        ),
        DeclareLaunchArgument(
            "tracker_min_turning_radius",
            default_value="0.25",
            description="Pure Pursuit curvature limit in meters.",
        ),
        DeclareLaunchArgument(
            "lookahead_distance",
            default_value="0.28",
            description="Pure Pursuit lookahead in meters.",
        ),
        DeclareLaunchArgument("min_lookahead_distance", default_value="0.10"),
        DeclareLaunchArgument("lookahead_curvature_gain", default_value="0.80"),
        DeclareLaunchArgument("lookahead_error_gain", default_value="2.00"),
        DeclareLaunchArgument("heading_correction_gain", default_value="0.80"),
        DeclareLaunchArgument("cross_track_correction_gain", default_value="1.20"),
        DeclareLaunchArgument("cross_track_speed_gain", default_value="1.80"),
        DeclareLaunchArgument("min_tracking_speed_ratio", default_value="0.35"),
        DeclareLaunchArgument("path_reacquire_distance", default_value="0.45"),
        DeclareLaunchArgument("linear_speed", default_value="0.45"),
        DeclareLaunchArgument("min_linear_speed", default_value="0.06"),
        DeclareLaunchArgument("reverse_speed", default_value="0.18"),
        DeclareLaunchArgument("slowdown_distance", default_value="0.45"),
        DeclareLaunchArgument("curvature_slowdown_gain", default_value="0.06"),
        DeclareLaunchArgument("min_curvature_speed_ratio", default_value="0.70"),
        DeclareLaunchArgument("min_goal_slowdown_ratio", default_value="0.35"),
        DeclareLaunchArgument("max_angular_speed", default_value="3.20"),
        DeclareLaunchArgument("goal_tolerance", default_value="0.06"),
        DeclareLaunchArgument("goal_yaw_tolerance", default_value="0.174533"),
        DeclareLaunchArgument("align_goal_yaw", default_value="false"),
        DeclareLaunchArgument("final_alignment_offset", default_value="0.25"),
        DeclareLaunchArgument("final_alignment_speed", default_value="0.06"),
        DeclareLaunchArgument(
            "require_goal_yaw",
            default_value="false",
            description="Require Hybrid A* to match the goal yaw.",
        ),
        DeclareLaunchArgument(
            "planner_goal_xy_tolerance",
            default_value="0.10",
            description="Hybrid A* final XY tolerance in meters.",
        ),
        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(
            "use_grid_heuristic",
            default_value="true",
            description="Use obstacle-aware 2D grid distance as the Hybrid A* heuristic.",
        ),
        DeclareLaunchArgument(
            "heuristic_weight",
            default_value="1.35",
            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(
            "planner_resolution",
            default_value="0.10",
            description="Hybrid A* search grid resolution in meters.",
        ),
        DeclareLaunchArgument(
            "yaw_bins",
            default_value="36",
            description="Number of discrete Hybrid A* heading states.",
        ),
        DeclareLaunchArgument(
            "primitive_length",
            default_value="0.12",
            description="Length of each Hybrid A* motion primitive in meters.",
        ),
        DeclareLaunchArgument(
            "max_iterations",
            default_value="200000",
            description="Maximum Hybrid A* state expansions per plan.",
        ),
        DeclareLaunchArgument(
            "planning_timeout_ms",
            default_value="1500.0",
            description="Hybrid A* planning time limit in milliseconds.",
        ),
        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,
        map_server,
        lifecycle_manager,
        odom_map_tf,
        planner_node,
        tracker_node,
    ])
