import os
import subprocess
import yaml

from launch import LaunchDescription
from launch_ros.actions import Node, SetParameter
from launch.actions import IncludeLaunchDescription, RegisterEventHandler, LogInfo, ExecuteProcess, ExecuteLocal, TimerAction
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.event_handlers import OnProcessExit
from launch.launch_description_entity import LaunchDescriptionEntity
from pathlib import Path


def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, 'r') as file:
            print("Opened file")
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        print("ERROR: load_file " + package_name + " | " + file_path + " -> " + absolute_file_path)
        return None


def load_xacro(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    process = subprocess.run(["xacro", absolute_file_path], capture_output=True)
    if process.returncode != 0:
        print(process.stderr)
    return process.stdout.decode("UTF-8")


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, 'r') as file:
            print("Opened file")
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        print("ERROR: load_yaml " + package_name + " | " + file_path + " -> " + absolute_file_path)

        return None


def launch_chain(nodes: list[ExecuteLocal]):
    generatedNodes: list[LaunchDescriptionEntity] = []
    for index in range(1, len(nodes)):
        generatedNodes.append(RegisterEventHandler(
            OnProcessExit(
                target_action=nodes[index-1],
                on_exit=nodes[index]
            )
        ))
    generatedNodes.append(nodes[0])
    return generatedNodes


def generate_launch_description():

    robot_description_config = load_xacro('iisy_config', 'urdf/iisy.urdf')
    robot_description = {'robot_description': robot_description_config}

    robot_description_semantic_config = load_file('iisy_config', 'srdf/iisy.srdf')
    robot_description_semantic = {'robot_description_semantic': robot_description_semantic_config}

    kinematics_yaml = load_yaml('iisy_config', 'config/kinematics.yml')
    robot_description_kinematics = {'robot_description_kinematics': kinematics_yaml}

    controllers = {
        "moveit_simple_controller_manager": load_yaml("iisy_config", "config/iisy_controllers.yml"),
        "moveit_controller_manager": "moveit_ros_control_interface/Ros2ControlMultiManager",
    }

    ompl_planning_pipeline_config = {
        'move_group': {
            'planning_plugin': 'ompl_interface/OMPLPlanner',
            'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
            'start_state_max_bounds_error': 0.1
        }
    }
    ompl_planning_yaml = load_yaml('iisy_config', 'config/ompl_planning.yml')
    ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)

    robot_description_planning = {
        "robot_description_planning": PathJoinSubstitution(
            [
                FindPackageShare("iisy_config"),
                "config/joint_limits.yml"
            ]
        )
    }

    trajectory_execution = {
        'moveit_manage_controllers': False,
        'trajectory_execution.allowed_execution_duration_scaling': 1.2,
        'trajectory_execution.allowed_goal_duration_margin': 0.5,
        'trajectory_execution.allowed_start_tolerance': 0.01
    }

    planning_scene_monitor_parameters = {
        "publish_planning_scene": True,
        "publish_geometry_updates": True,
        "publish_state_updates": True,
        "publish_transforms_updates": True
    }

    planning = {
        "move_group": {
            "planning_plugin": "ompl_interface/OMPLPlanner",
            "request_adapters": "default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
            "start_state_max_bounds_error": 0.1
        }
    }

    spawn_robot = Node(
        package='ros_gz_sim',
        executable='create',
        arguments=['-world', 'default', '-string', robot_description_config],
        output='screen'
    )

    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', os.path.join(get_package_share_directory("ign_world"), 'rviz', 'iisy.rviz')],
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_planning,
            ompl_planning_pipeline_config,
            robot_description_kinematics,
            planning,
            trajectory_execution,
            controllers,
            planning_scene_monitor_parameters,
        ],
    )

    controller_manager = Node(
            package="controller_manager",
            executable="ros2_control_node",
            parameters=[
                robot_description,
                str(Path(get_package_share_directory("iisy_config")+"/config/iisy_controllers.yaml")),
            ],
    )

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

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

    move_group = Node(
        package='moveit_ros_move_group',
        executable='move_group',
        # prefix='xterm -fs 10 -e gdb --ex run --args',
        output='both',
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_planning,
            ompl_planning_pipeline_config,
            robot_description_kinematics,
            planning,
            trajectory_execution,
            controllers,
            planning_scene_monitor_parameters,
            # octomap_config,
            # octomap_updater_config
        ]
    )

    test = Node(
        package='moveit_test',
        executable='move',
        output='both',
        parameters=[
            robot_description,
            robot_description_semantic,
            robot_description_planning,
            ompl_planning_pipeline_config,
            robot_description_kinematics,
            planning,
            trajectory_execution,
            controllers,
            planning_scene_monitor_parameters,
            # octomap_config,
            # octomap_updater_config
        ],
    )

    # octomap_config = {'octomap_frame': 'camera_rgb_optical_frame',
    #                  'octomap_resolution': 0.05,
    #                  'max_range': 5.0}

    # octomap_updater_config = load_yaml('moveit_tutorials', 'config/sensors_3d.yaml')

    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')

    return LaunchDescription([

        #IncludeLaunchDescription(
        #    PythonLaunchDescriptionSource(os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
        #    launch_arguments={'gz_args': '-r '+get_package_share_directory('ign_world')+'/world/gaz_new_test.sdf'}.items(),
        #),

        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),

        move_group,

        #RegisterEventHandler(
        #    OnProcessExit(
        #        target_action=spawn_robot,
        #        on_exit=[load_joint_state_broadcaster]
        #    )
        #),

        controller_manager,

        RegisterEventHandler(
            OnProcessExit(
                target_action=load_joint_state_broadcaster,
                on_exit=[load_joint_trajectory_controller]
            )
        ),
        RegisterEventHandler(
            OnProcessExit(
                target_action=load_joint_trajectory_controller,
                on_exit=TimerAction(period=10.0,actions=[test])
            )
        ),

        #spawn_robot,
        load_joint_state_broadcaster,
        #load_joint_trajectory_controller,
        rviz,

        Node(
            package='ros_actor_action_server',
            executable='ros_actor_action_server'),

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='lidar_1_broadcaster',
            arguments=[
                "--x", "1",
                "--y", "-1.9",
                "--z", "1",
                "--roll", "1.5707963267949",
                "--pitch", "0",
                "--yaw", "0",
                "--frame-id", "world",
                "--child-frame-id", "lidar_1_link"]
        ),

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            name='lidar_2_broadcaster',
            arguments=[
                "--x", "-1.9",
                "--y", "1",
                "--z", "1",
                "--roll", "0",
                "--pitch", "0",
                "--yaw", "0",
                "--frame-id", "world",
                "--child-frame-id", "lidar_2_link"]
        ),

        #Node(
        #    package='ros_gz_bridge',
        #    executable='parameter_bridge',
        #    arguments=["/world/default/model/iisy/joint_state[sensor_msgs/msg/JointState@ignition.msgs.Model"],
        #    output='both',
        #    remappings=[
        #        ('/world/default/model/iisy/joint_state', '/joint_states'),
        #    ]
        #),

        Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            output='both',
            parameters=[robot_description]
        ),
    ])