#!/usr/bin/env python

# Copyright 1996-2023 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""Launch Webots Universal Robot simulation nodes."""

import os
import launch
from launch_ros.actions import Node
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.urdf_spawner import URDFSpawner, get_webots_driver_node
from webots_ros2_driver.webots_controller import WebotsController
from rbs_utils.launch import load_xacro_args
from robot_builder.external.ros2_control import ControllerManager
from robot_builder.parser.urdf import URDF_parser
import xacro


PACKAGE_NAME = 'aubo_description'


def generate_launch_description():
    package_dir = get_package_share_directory(PACKAGE_NAME)
    robot_description_path = os.path.join(package_dir, 'urdf', 'aubo_i5.urdf.xacro')
    controllers_file = os.path.join(package_dir, 'config', 'controllers.yaml')

    mappings_data = load_xacro_args(robot_description_path, locals())

    robot_description_doc = xacro.process_file(
        robot_description_path, mappings={"robot_ip": "", "simulation_controllers": controllers_file, "hardware": "webots", "with_gripper": "false"}
    )

    robot_description_content = robot_description_doc.toprettyxml(indent="  ")
    # with open("current.urdf", "w") as file:
    #     file.write(robot_description_content)
    #     file.close()

    robot = URDF_parser.load_string(
        robot_description_content,
        base_link_name="base_link",
        ee_link_name="wrist3_Link",
    )
    ControllerManager.save_to_yaml(
        robot, package_dir, "controllers.yaml"
    )

    # Define your URDF robots here
    # The name of an URDF robot has to match the name of the robot of the driver node
    # You can specify the URDF file to use with "urdf_path"
    spawn_URDF_aubo = URDFSpawner(
        name='Aubo-i5',
        robot_description=robot_description_content,
        translation='0 0 0.6',
        rotation='0 0 1 -1.5708',
    )

    # Driver nodes
    # When having multiple robot it is mandatory to specify the robot name.
    aubo_robot_driver = WebotsController(
        robot_name='Aubo-i5',
        parameters=[
            {'robot_description': robot_description_content},
            {'use_sim_time': True},
            {'set_robot_state_publisher': True},
            controllers_file
        ],
    )

    # Other ROS 2 nodes
    controller_manager_timeout = ['--controller-manager-timeout', '100']
    trajectory_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        arguments=['cartesian_motion_controller'] + controller_manager_timeout,
    )

    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        output='screen',
        arguments=['joint_state_broadcaster'] + controller_manager_timeout,
    )

    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[{
            'robot_description': '<robot name=""><link name=""/></robot>'
        }],
    )

    return LaunchDescription([
        # Request to spawn the URDF robot
        spawn_URDF_aubo,

        # Other ROS 2 nodes
        robot_state_publisher,
        trajectory_controller_spawner,
        joint_state_broadcaster_spawner,

        # Launch the driver node once the URDF robot is spawned
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessIO(
                target_action=spawn_URDF_aubo,
                on_stdout=lambda event: get_webots_driver_node(event, aubo_robot_driver),
            )
        ),

        # Kill all the nodes when the driver node is shut down
        launch.actions.RegisterEventHandler(
            event_handler=launch.event_handlers.OnProcessExit(
                target_action=aubo_robot_driver,
                on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
            )
        ),
    ])