# 1. launch multi navigation.
# 2. run this test.


from main_engine_client import IF as Engine
import time
from pyrois.RoIS_Common import Component_Status
from pyrois.Service_Application_IF import Service_Application_IF
from utilities import setup_multi_robot
import xmlrpc


class Service(Service_Application_IF):
    def __init__(self, uri="http://localhost:8000"):
        super().__init__(uri)
        self.current_dest_index = 0
        self.is_running = False
        self.robot_positions = []
        self.commandid_task_table = {}

    def completed(self, command_id, status):
        print("Service command_id {} was completed.".format(command_id))
        if self.commandid_task_table[command_id] == 'Navigation':
            self.current_dest_index += 1
            self.is_running = False
        else:  # task 'RobotPosition'
            status, results = self._proxy.get_command_result(command_id, None)
            self.robot_positions = results

    def go_robot_to(self, dest):
        self._proxy = xmlrpc.client.ServerProxy(self._uri)
        time.sleep(3)
        (return_code, command_id) = self._proxy.set_parameter('Navigation', dest, "", "")
        self.commandid_task_table[command_id] = 'Navigation'
        print("go_robot_to {} called with command_id {}".format(dest, command_id))
        self.is_running = True

    def update_robot_positions(self):
        self._proxy = xmlrpc.client.ServerProxy(self._uri)
        time.sleep(3)
        (return_code, command_id) = self._proxy.set_parameter('RobotPosition')
        self.commandid_task_table[command_id] = 'RobotPosition'
        time.sleep(2)
        status, results = self._proxy.get_command_result(command_id, "")
        self.robot_positions = results

    def get_robot_positions(self):
        return self.robot_positions

    def speech_recognition(self, robot_name):
        self._proxy = xmlrpc.client.ServerProxy(self._uri)
        time.sleep(3)
        (return_code, command_id) = self._proxy.set_parameter('Speech_Recognition', "English", "", "")
        time.sleep(15)


    def run(self):
        destinations = [
            "point_b", "point_h", [5, 0, 0], "point_e", "point_a", [0, 5, 0]
        ]
        self.speech_recognition("robot3")
        for dest in destinations:
            self.go_robot_to(dest)
            while self.is_running:
                #  self.update_robot_positions()
                #  robot_positions = self.get_robot_positions()
                #  print("robot_positions:")
                #  [print(f"\t{i}: {pos}") for i, pos in enumerate(robot_positions)]
                time.sleep(3)


if __name__ == '__main__':
    process = setup_multi_robot()
    time.sleep(10)
    service = Service()
    service.run()
    print("finish")
