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"])

    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"
    )

    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"]
    )
    
    # forward_position_controller_spawner = Node(
    #     package = "controller_manager",
    #     executable = "spawner",
    #     arguments = ["forward_position_controller", "--controller-manager", "/controller_manager"]
    # )

    joint_trajectory_controller_spawner = Node(
        package = "controller_manager",
        executable = "spawner",
        arguments = ["joint_trajectory_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_forward_position_controller_spawner_after_joint_state_broadcaster_spawner = RegisterEventHandler(
    #     event_handler = OnProcessExit(
    #         target_action = joint_state_broadcaster_spawner,
    #         on_exit = [forward_position_controller_spawner]
    #     )
    # )

    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_rviz_after_joint_state_broadcaster_spawner,
        # delay_forward_position_controller_spawner_after_joint_state_broadcaster_spawner,
        delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
    ]
            
    return LaunchDescription(nodes)