from launch import LaunchDescription
from launch.actions import RegisterEventHandler
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.event_handlers import OnProcessExit


from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

    robot_description_content = Command([
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            PathJoinSubstitution(
                [
                    FindPackageShare("hexapod_robot"),
                    "config",
                    "urdf",
                    "hexapod_robot.urdf.xacro"
                ]
            )
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "ros2_controllers.yaml"])

    robot_state_publisher_node = Node(
        package = "robot_state_publisher",
        executable = "robot_state_publisher",
        parameters = [robot_description],
        output = "both"
    )            

    control_node = Node(
        package = "controller_manager",
        executable = "ros2_control_node",
        parameters = [robot_description, robot_controllers],
        output="both"
    )

    joint_state_broadcaster_spawner = Node (
        package = "controller_manager",
        executable = "spawner",
        arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
    )

    joint_trajectory_controller_spawner = Node(
        package = "controller_manager",
        executable = "spawner",
        arguments = ["e1_group_controller", "e2_group_controller", "e3_group_controller", "e4_group_controller", "e5_group_controller", "e6_group_controller", "--controller-manager", "/controller_manager"]
    )


    delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler = OnProcessExit(
            target_action = joint_state_broadcaster_spawner,
            on_exit = [joint_trajectory_controller_spawner]
        )
    )

    nodes = [
        control_node,
        robot_state_publisher_node,
        joint_state_broadcaster_spawner,
        delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
    ]
            
    return LaunchDescription(nodes)