from launch.actions.declare_launch_argument import DeclareLaunchArgument
from launch.launch_description import LaunchDescription
from launch.actions import IncludeLaunchDescription, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
from launch.substitutions.launch_configuration import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro
import os
from ament_index_python import get_package_share_directory

def generate_launch_description():

    # Launch arguments
    launch_args = []

    launch_args.append(DeclareLaunchArgument(
            name="robot_name",
            default_value="rasms",
            description="Set robot name."
        )
    )

    launch_args.append(DeclareLaunchArgument(
            name="sim",
            default_value="true",
            description="Launch robot in simulation or on real setup."
        )
    )
    # get xacro file path
    xacro_file = os.path.join(get_package_share_directory("rasms_description"),"urdf/","rasms_description.urdf.xacro")
    # get error if xacro file if missing
    assert os.path.exists(xacro_file), "The xacro file of rasms_description doesnt exist"+str(xacro_file)
    # parse xacro file from file
    robot_description = xacro.process_file(xacro_file)
    # convert file to xml format
    robot_description_content = robot_description.toxml()

    # Load controls
    control = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("rasms_moveit_config"),
                "launch",
                "rasms_control.launch.py"
            ])
        ), launch_arguments=[
            ("robot_description", robot_description_content)
        ]
    )

    # Gazebo simulation 
    simulation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
               FindPackageShare("rasms_moveit_config"),
                "launch",
                "rasms_simulation.launch.py"
            ])
        ), 
        launch_arguments=[
           ("robot_name", LaunchConfiguration("robot_name"))
        ],
        condition=IfCondition(LaunchConfiguration("sim"))
    )

    # Move group
    move_group = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            PathJoinSubstitution([
                FindPackageShare("rasms_moveit_config"),
                "launch",
                "rasms_moveit_interface.launch.py"
            ])
        ), 
        launch_arguments=[
            ("robot_description", robot_description_content),
            ("sim", LaunchConfiguration("sim"))
        ]
    )

    launch_nodes = []
    launch_nodes.append(control)
    launch_nodes.append(simulation)
    launch_nodes.append(move_group)

    

    return LaunchDescription(
        launch_args +
        launch_nodes
    )