#!/usr/bin/python3.6

import rclpy
from rclpy.node import Node
import sys
from random import choice
from string import ascii_letters
from rcl_interfaces.srv import SetParameters

rclpy.init(args=None)
node = Node('set_parameters_' + ''.join(choice(ascii_letters) for _ in range(8)),
            automatically_declare_parameters_from_overrides = True)

target_node = node.get_parameter('simple_launch.node').value
# get which parameters are to be set
params = node.get_parameter('simple_launch.keys').value

if not target_node:
    sys.exit(0)

if target_node[0] != '/' and len(node.get_namespace()) > 1:
    target_node = f'{node.get_namespace()}/{target_node}'
if target_node[0] != '/':
    target_node = f'/{target_node}'

# wait for set param target_node
param_client = node.create_client(SetParameters, target_node + '/set_parameters')
param_client.wait_for_service()

# declare and get them values
target_params = [node.get_parameter(name) for name in params]
verbosity = node.get_parameter('simple_launch.verbosity').value

req_v = 'req' in verbosity
res_v = 'res' in verbosity

if req_v:
    node.get_logger().info(f'Setting parameters for {target_node}: {", ".join(params)}')

# convert to SetParam request and call
request = SetParameters.Request()
request.parameters = [param.to_parameter_msg() for param in target_params]

res = param_client.call_async(request)

while not res.done():
    rclpy.spin_once(node)

failed = []
success = []
for i,result in enumerate(res.result().results):

    if result.successful:
        success.append(params[i])
    else:
        failed.append('Set parameter failed')
        if result.reason:
            failed[-1] += f': {result.reason.strip(".")} (was {target_params[i].value})'

out = []
if success and req_v:
    out.insert(0, 'Set parameter successful: ' + ', '.join(success))
# print failures anyway
out += failed

if out:
    for param in out:
        node.get_logger().info(param)

node.destroy_node()
rclpy.shutdown()
