import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler, TimerAction
from launch.event_handlers import OnProcessExit
from launch_ros.actions import Node, SetParameter
from launch.launch_description_sources import PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder
from launch.substitutions import LaunchConfiguration, TextSubstitution
from launch.conditions import LaunchConfigurationEquals
import xacro


def generate_launch_description():
    scene_launch_arg = DeclareLaunchArgument(
        "scene", default_value=TextSubstitution(text="Coex")
    )

    worldFile = open('/tmp/gazebo_world.sdf', 'w')
    worldFile.write(xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'world.sdf')))
    worldFile.flush()
    worldFile.close()

    scene = LaunchConfiguration('scene')

    moveit_config = MoveItConfigsBuilder("iisy", package_name="iisy_config").to_moveit_configs()

    load_joint_state_broadcaster = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'],
        output='screen'
    )

    load_joint_trajectory_controller = ExecuteProcess(
        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'arm_controller'],
        output='screen'
    )

    ignition_spawn_entity = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=[
            '-name', 'iisy',
            '-allow_renaming', 'true',
            '-string', xacro.process(os.path.join(get_package_share_directory('iisy_config'), 'config', 'iisy.urdf.xacro')),
        ],
        output='screen'
    )

    btree = Node(
        package='btree',
        executable='tree',
        output='both',
        parameters=[
            {
                "trees": [
                    [get_package_share_directory('btree'), '/trees/actorTree', scene, '.xml'],
                    [get_package_share_directory('btree'), '/trees/robotTree', scene, '.xml'],
                ],
                "areas": [
                    "safeArea",
                    "1,   3.5 |" +
                    "1,   7   |" +
                    "3,   3.5 |" +
                    "7,   7   |" +
                    "3,  -1   |" +
                    "7,  -1",
                    "warningArea",
                    "-1,  2.5 |" +
                    "-1,  3.5 |" +
                    "2,   2.5 |" +
                    "3,   3.5 |" +
                    "2,  -1   |" +
                    "3,  -1",
                    "unsafeArea",
                    "-1,  1.5 |" +
                    "-1,  2.5 |" +
                    "1,   1.5 |" +
                    "2,   2.5 |" +
                    "1,  -1   |" +
                    "2,  -1",
                    "negativeYTable",
                    "0.3,  -0.25 |" +
                    "-0.3, -0.25 |" +
                    "0,    -0.4",
                    "positiveYTable",
                    "0.3,   0.25 |" +
                    "-0.3,  0.25 |" +
                    "0,     0.4"
                ]
            },
        ],
        emulate_tty=True,
    )

    return LaunchDescription([
        scene_launch_arg,

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
            launch_arguments={'gz_args': '-v 4 -r /tmp/gazebo_world.sdf'}.items(),
        ),

        Node(
            package='ros_gz_sim',
            executable='create',
            arguments=[
                '-name', 'conveyor',
                '-allow_renaming', 'true',
                '-string', xacro.process(os.path.join(get_package_share_directory('ign_world'), 'world', 'conveyor.sdf')),
                # '-file', 'https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Gazebo',
            ],
            output='screen',
            condition=LaunchConfigurationEquals('scene', 'Coop')
        ),

        Node(
            package='ros_gz_bridge',
            executable='parameter_bridge',
            arguments=["/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock"],
            output='both',
        ),

        SetParameter(name="use_sim_time", value=True),

        Node(
            package='ros_actor_action_server',
            executable='ros_actor_action_server',
            output='both',
            emulate_tty=True,
        ),
        
        TimerAction(
            period=10.0,
            actions=[
                btree,
            ]
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                str(moveit_config.package_path / "launch/rsp.launch.py")
            ),
        ),

        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                str(moveit_config.package_path / "launch/move_group.launch.py")
            ),
        ),

        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=ignition_spawn_entity,
                on_exit=[load_joint_state_broadcaster],
            )
        ),

        RegisterEventHandler(
            event_handler=OnProcessExit(
                target_action=load_joint_state_broadcaster,
                on_exit=[load_joint_trajectory_controller],
            )
        ),

        ignition_spawn_entity,
    ])
