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"),
                    "description",
                    "urdf",
                    "hexapod_robot.urdf.xacro"
                ]
            )
        ]
    )
    robot_description = {"robot_description": robot_description_content}

    robot_controllers = PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "controllers.yaml"])
    
    control_node = Node(
        package = "controller_manager",
        executable = "ros2_control_node",
        parameters = [robot_description, robot_controllers],
        output="both"
    )

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

    joint_state_publisher_gui = Node(
        package="joint_state_publisher_gui",
        executable="joint_state_publisher_gui"
    )
            
    rviz_node = Node(
        package = "rviz2",
        executable = "rviz2",
        arguments = ['-d', PathJoinSubstitution([FindPackageShare("hexapod_robot"), "config", "hexapod_robot.rviz"])],
        name = "rviz2",
        output = "screen",
    )

    joint_state_broadcaster_spawner = Node (
        package = "controller_manager",
        executable = "spawner",
        arguments = ["joint_state_broadcaster", "--controller-manager", "/controller_manager"]
    )
    
    robot_controller_spawner = Node(
        package = "controller_manager",
        executable = "spawner",
        arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
    )
    
    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler = OnProcessExit(
            target_action = joint_state_broadcaster_spawner,
            on_exit = [rviz_node]
        )
    )
    
    delay_robot_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler = OnProcessExit(
            target_action = joint_state_broadcaster_spawner,
            on_exit = [robot_controller_spawner]
        )
    )
    
    nodes = [
        control_node,
        robot_state_publisher_node,
        joint_state_broadcaster_spawner,
        delay_rviz_after_joint_state_broadcaster_spawner,
        delay_robot_controller_spawner_after_joint_state_broadcaster_spawner        
    ]
            
    return LaunchDescription(nodes)