# Copyright 2018 Open Source Robotics Foundation, Inc.
#
# 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.

import sys

from rcl_interfaces.msg import ParameterType
import rclpy
from rclpy.parameter import parameter_dict_from_yaml_file
from rclpy.parameter import parameter_value_to_python
from rclpy.parameter_client import AsyncParameterClient
from ros2cli.node.direct import DirectNode


def get_value(*, parameter_value):
    """Get the value from a ParameterValue."""
    value = None
    try:
        value = parameter_value_to_python(parameter_value)
    except RuntimeError as e:
        print(f'Runtime error {e} raised')
    return value


def load_parameter_file(*, node, node_name, parameter_file, use_wildcard, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        raise RuntimeError('Wait for service timed out waiting for '
                           f'parameter services for node {node_name}')
    future = client.load_parameter_file(parameter_file, use_wildcard)
    parameters = list(parameter_dict_from_yaml_file(
        parameter_file, use_wildcard, target_nodes=[node_name]).values())
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    if not future.done():
        raise RuntimeError(
            f'Timed out waiting for load parameter file response from node {node_name} '
            f'(timeout: {timeout}s)')
    response = future.result()
    assert len(response.results) == len(parameters), 'Not all parameters set'
    for i in range(0, len(response.results)):
        result = response.results[i]
        param_name = parameters[i].name
        if result.successful:
            msg = 'Set parameter {} successful'.format(param_name)
            if result.reason:
                msg += ': ' + result.reason
            print(msg)
        else:
            msg = 'Set parameter {} failed'.format(param_name)
            if result.reason:
                msg += ': ' + result.reason
            print(msg, file=sys.stderr)


def call_describe_parameters(*, node, node_name, parameter_names=None, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        raise RuntimeError('Wait for service timed out waiting for '
                           f'parameter services for node {node_name}')
    future = client.describe_parameters(parameter_names)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    if not future.done():
        raise RuntimeError(
            f'Timed out waiting for describe_parameters response from node {node_name} '
            f'(timeout: {timeout}s)')
    response = future.result()
    if response is None:
        raise RuntimeError('Exception while calling service of node '
                           f'{node_name}: {future.exception()}')
    return response


def call_get_parameters(*, node, node_name, parameter_names, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        raise RuntimeError('Wait for service timed out waiting for '
                           f'parameter services for node {node_name}')
    future = client.get_parameters(parameter_names)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    if not future.done():
        raise RuntimeError(
            f'Timed out waiting for get_parameters response from node {node_name} '
            f'(timeout: {timeout}s)')
    response = future.result()
    if response is None:
        raise RuntimeError('Exception while calling service of node '
                           f'{node_name}: {future.exception()}')
    return response


def call_set_parameters(*, node, node_name, parameters, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        raise RuntimeError('Wait for service timed out waiting for '
                           f'parameter services for node {node_name}')
    future = client.set_parameters(parameters)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    if not future.done():
        raise RuntimeError(
            f'Timed out waiting for set_parameters response from node {node_name} '
            f'(timeout: {timeout}s)')
    response = future.result()
    if response is None:
        raise RuntimeError('Exception while calling service of node '
                           f'{node_name}: {future.exception()}')
    return response


def call_set_parameters_atomically(*, node, node_name, parameters, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        raise RuntimeError('Wait for service timed out waiting for '
                           f'parameter services for node {node_name}')
    future = client.set_parameters_atomically(parameters)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    if not future.done():
        raise RuntimeError(
            f'Timed out waiting for set_parameters_atomically response from node {node_name} '
            f'(timeout: {timeout}s)')
    response = future.result()
    if response is None:
        raise RuntimeError('Exception while calling service of node '
                           f'{node_name}: {future.exception()}')
    return response


def call_list_parameters(*, node, node_name, prefixes=None, timeout=None):
    client = AsyncParameterClient(node, node_name)
    ready = client.wait_for_services(timeout_sec=5.0)
    if not ready:
        return None
    future = client.list_parameters(prefixes=prefixes)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    return future


def get_parameter_type_string(parameter_type):
    mapping = {
        ParameterType.PARAMETER_BOOL: 'boolean',
        ParameterType.PARAMETER_INTEGER: 'integer',
        ParameterType.PARAMETER_DOUBLE: 'double',
        ParameterType.PARAMETER_STRING: 'string',
        ParameterType.PARAMETER_BYTE_ARRAY: 'byte array',
        ParameterType.PARAMETER_BOOL_ARRAY: 'boolean array',
        ParameterType.PARAMETER_INTEGER_ARRAY: 'integer array',
        ParameterType.PARAMETER_DOUBLE_ARRAY: 'double array',
        ParameterType.PARAMETER_STRING_ARRAY: 'string array',
        ParameterType.PARAMETER_NOT_SET: 'not set',
    }
    return mapping[parameter_type]


class ParameterNameCompleter:
    """Callable returning a list of parameter names."""

    def __call__(self, prefix, parsed_args, **kwargs):
        if not parsed_args.node_name:
            return []
        with DirectNode(parsed_args) as node:
            future = call_list_parameters(
                node=node, node_name=parsed_args.node_name)
            if future is None:
                return []
            result = future.result()
            if result is None:
                return []
            parameter_names = result.result.names
            return [
                n for n in parameter_names
                if not prefix or n.startswith(prefix)]
