import os

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, Command
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare

namespace = 'fanuc_1'


def generate_launch_description():
    # Set the path to this package.
    pkg_share = FindPackageShare(package='fanuc_crx_description').find('fanuc_crx_description')

    # Set the path to the URDF file
    urdf_model_path = os.path.join(pkg_share, 'robots/crx10ia.xacro')

    position_x = LaunchConfiguration('x')
    position_y = LaunchConfiguration('y')
    position_z = LaunchConfiguration('z')
    orientation_roll = LaunchConfiguration('roll')
    orientation_pitch = LaunchConfiguration('pitch')
    orientation_yaw = LaunchConfiguration('yaw')

    declare_x_position = DeclareLaunchArgument(
        name='x',
        default_value="0",
        description='Robot position in x direction'
    )

    declare_y_position = DeclareLaunchArgument(
        name='y',
        default_value="0",
        description='Robot position in y direction'
    )

    declare_z_position = DeclareLaunchArgument(
        name='z',
        default_value="0",
        description='Robot position in z direction'
    )

    declare_roll_orientation = DeclareLaunchArgument(
        name='roll',
        default_value="0",
        description='Robot orientation in roll'
    )

    declare_pitch_orientation = DeclareLaunchArgument(
        name='pitch',
        default_value="0",
        description='Robot orientation in pitch'
    )

    declare_yaw_orientation = DeclareLaunchArgument(
        name='yaw',
        default_value="0",
        description='Robot orientation in yaw'
    )

    start_robot_state_publisher_cmd = Node(
        package='robot_state_publisher',
        namespace=namespace,
        executable='robot_state_publisher',
        parameters=[{'frame_prefix': namespace + '/',
                     'robot_description': Command(['xacro ', urdf_model_path])}],
        arguments=[urdf_model_path]
    )

    start_static_transform_publisher = Node(package="tf2_ros",
                                            executable="static_transform_publisher",
                                            arguments=[position_x,
                                                       position_y,
                                                       position_z,
                                                       orientation_roll,
                                                       orientation_pitch,
                                                       orientation_yaw,
                                                       "map",
                                                       namespace + "/base_link"])

    ld = LaunchDescription()

    ld.add_action(declare_x_position)
    ld.add_action(declare_y_position)
    ld.add_action(declare_z_position)
    ld.add_action(declare_roll_orientation)
    ld.add_action(declare_pitch_orientation)
    ld.add_action(declare_yaw_orientation)
    ld.add_action(start_robot_state_publisher_cmd)
    ld.add_action(start_static_transform_publisher)

    return ld
