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

from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():

    moveit_config = (
        MoveItConfigsBuilder("hexapod_robot", package_name = "hexapod_robot")

        .planning_scene_monitor(
            publish_robot_description=True, publish_robot_description_semantic=True
        )        
        .planning_pipelines(pipelines = ["ompl"])
        .to_moveit_configs()
    )

    move_group_node = Node(
        package = "moveit_ros_move_group",
        executable = "move_group",
        output = "screen",
        parameters = [moveit_config.to_dict()]
    )

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

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

    ros2_control_node = Node(
        package = "controller_manager",
        executable = "ros2_control_node",
        parameters = [moveit_config.robot_description, ros2_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_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
        event_handler = OnProcessExit(
            target_action = joint_state_broadcaster_spawner,
            on_exit = [rviz_node]
        )
    )


    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 = [
        robot_state_publisher_node,
        move_group_node,
        ros2_control_node,
        joint_state_broadcaster_spawner,
        delay_joint_trajectory_controller_spawner_after_joint_state_broadcaster_spawner
    ]
            
    return LaunchDescription(nodes)