# HRI_Engine_example.py
#
# Copyright 2017 Eiichi Inohira
# This software may be modified and distributed under the terms
# of the MIT license
#
# For python 3
# For HRI Engine

"""HRI_Engine_example
"""

from __future__ import print_function

import os
import sys
import queue
import numpy as np
import threading

from pyrois import RoIS_HRI
from pyrois.Service_Application_Base_example import Service_Application_Base

if sys.version_info.major == 2:
    import SocketServer
    import SimpleXMLRPCServer

    class ThreadingXMLRPCServer(SocketServer.ThreadingMixIn, SimpleXMLRPCServer.SimpleXMLRPCServer):
        """ThreadingXMLRPCServer
        """
        pass
else:
    import socketserver
    import xmlrpc.server

    class ThreadingXMLRPCServer(socketserver.ThreadingMixIn, xmlrpc.server.SimpleXMLRPCServer):
        """ThreadingXMLRPCServer
        """
        pass


class SystemIF(RoIS_HRI.SystemIF):
    """SystemIF
    """
    def __init__(self, Engine):
        self._engine = Engine

    def connect(self):
        # print("connect")
        if self._engine.state is False:
            self._engine.state = True
            status = RoIS_HRI.ReturnCode_t.OK.value
        else:
            status = RoIS_HRI.ReturnCode_t.ERROR.value
        # time.sleep(30)
        return status

    def disconnect(self):
        if self._engine.state is True:
            self._engine.state = False
            status = RoIS_HRI.ReturnCode_t.OK.value
        else:
            status = RoIS_HRI.ReturnCode_t.ERROR.value
        #status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def get_profile(self, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        # RoIS_HRI_Profile
        profile = "Unsupported"
        return (status, profile)

    def get_error_detail(self, error_id, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        # ResultLists
        results = "None"
        return (status, results)


class CommandIF(RoIS_HRI.CommandIF):
    """CommandIF
    """
    def __init__(self, Engine):
        self._engine = Engine

    def search(self, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        # List< RoIS_Identifier>
        component_ref_list = ["None"]
        return (status, component_ref_list)

    def bind(self, component_ref):
        # print("state:", self._engine.state)
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def bind_any(self, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        component_ref_list = ["None"]
        return (status, component_ref_list)

    def release(self, component_ref):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def get_parameter(self, component_ref):
        status = RoIS_HRI.ReturnCode_t.OK.value
        parameter_list = ["None"]
        return (status, parameter_list)

    def set_parameter(self, component_ref, parameters):
        status = RoIS_HRI.ReturnCode_t.OK.value
        command_id = "0"
        return (status, command_id)

    def execute(self, command_unit_list):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def get_command_result(self, command_id, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        results = ["None"]
        return (status, results)


class QueryIF(RoIS_HRI.QueryIF):
    """
    class QueryIF(object):
    """

    def __init__(self, Engine):
        self._engine = Engine

    def query(self, query_type, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        results = ["None"]
        return (status, results)


class EventIF(RoIS_HRI.EventIF):
    """
    class QueryIF(object):
    """

    def __init__(self, Engine):
        self._engine = Engine

    def subscribe(self, event_type, condition):
        """
        subscribe(self, event_type, condition) -> (status,subscribe_id)
        """
        status = RoIS_HRI.ReturnCode_t.OK.value
        subscribe_id = "0"
        return (status, subscribe_id)

    def unsubscribe(self, subscribe_id):
        """
        unsubscribe(self,subscribe_id) -> status
        """
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def get_event_detail(self, event_id, condition):
        """
        get_event_detail(self,event_id,condition) -> (status,results)
        """
        status = RoIS_HRI.ReturnCode_t.OK.value
        results = ["None"]
        return (status, results)

from VirtualNavigation_client import VirtualNavigation_Client as VNavClient
from Dummy_Speech_Recognition_client import Speech_Recognition_Client as SRecogClient
from Person_Localization_client import Person_Localization_Client as PLClient
from sub_engine_client import IF as EngineClient
from pyrois.RoIS_Common import Component_Status
from pyrois.RoIS_Service import Completed_Status
import time
from termcolor import colored

class IF(SystemIF, CommandIF, QueryIF, EventIF, Service_Application_Base):
    """IF
    """
    def __init__(self, Engine):
        super().__init__(Engine)
        self.command_id = 0
        self.event_id = 0
        self.subscribe_id = 0
        self.vnav_event_queue = queue.Queue()
        self.event_queue = queue.Queue()
        self.component_event_queue = queue.Queue()
        self.component_clients = {
            'Navigation': VNavClient('http://localhost:8041', self.vnav_event_queue),
            'Speech_Recognition1': SRecogClient('http://localhost:8013', self.component_event_queue),
            'Speech_Recognition2': SRecogClient('http://localhost:8023', self.component_event_queue),
            'Speech_Recognition3': SRecogClient('http://localhost:8033', self.component_event_queue),
            'Person_Localization': PLClient('http://localhost:8043', self.component_event_queue)
        }
        self.engine_clients = {
            'robot1': EngineClient('http://localhost:8010'),
            'robot2': EngineClient('http://localhost:8020'),
            'robot3': EngineClient('http://localhost:8030')
        }
        self.goal_command_table = {}
        for engine_client in self.engine_clients.values():
            engine_client.connect()
        self.command_id_result_table = {}
        self.event_ref_subscribe_id_table = {}
        self.event_id_result_table = {}
        self.sr_robot = "robot1"

        th = threading.Thread(target=self.analyze_vnav_status, daemon=True)
        th.start()
        th = threading.Thread(target=self.monitor_event, daemon=True)
        th.start()

    def set_parameter(self, component_ref, *parameters):
        self.command_id += 1
        if component_ref == 'Navigation':
            target_component = self.component_clients[component_ref]
            status = target_component.set_parameter(*parameters).value
            goal = parameters[0]
            self.goal_command_table[str(goal)] = str(self.command_id)
        elif component_ref == 'RobotPosition':
            self.command_id_result_table[str(self.command_id)] = []
            for engine_client in self.engine_clients.values():
                status, command_id = engine_client.set_parameter('System_Information')
                time.sleep(3)
                status, results = engine_client.get_command_result(command_id, "")
                self.command_id_result_table[str(self.command_id)].append(results)
            status = RoIS_HRI.ReturnCode_t.OK.value
        elif component_ref == 'Speech_Recognition':
            print("Speech_Recognition was called by set_parameter")
            #  target_component = self.component_clients[component_ref]
            #  status = target_component.set_parameter(*parameters).value

        return (status, str(self.command_id))

    def analyze_vnav_status(self):
        while True:
            msg = self.vnav_event_queue.get()
            params, methodname = xmlrpc.client.loads(msg)
            target_pos, robot_name, status = params
            command_id = self.goal_command_table[str(target_pos)]
            status = Component_Status(status)
            if status == Component_Status.READY:
                print(colored("Command (id: {}) was succeeded by {} which reached goal {}."\
                        .format(command_id, robot_name, target_pos), "green"))
                self.sr_robot = robot_name
                self.completed(command_id, Completed_Status.OK.value)
                del self.goal_command_table[str(target_pos)]
                time.sleep(3)
            elif status == Component_Status.ERROR:
                print(colored("Command (id: {}) was aborted by {} which couldn't reach goal {}."\
                        .format(command_id, robot_name, target_pos), "red"))
                self.completed(command_id, Completed_Status.ABORT.value)
            else:
                raise RuntimeError("Unhandled kind of vnav-status.")
            time.sleep(1)

    def monitor_event(self):
        while True:
            msg = self.component_event_queue.get()
            self.event_id += 1
            event_id = str(self.event_id)
            params, methodname = xmlrpc.client.loads(msg)
            #  print(f"Received event: methodname => {methodname}, params => {params}")
            if methodname == 'speech_recognized':
                timestamp, recognized_text, target_robot = params
                try:
                    subscribed_id = self.event_ref_subscribe_id_table['speech_recognized'+target_robot]
                except KeyError:
                    #  print("This is un-subscribed event. (speech_recognized)")
                    continue
                self.event_id_result_table[event_id] = params
                self.notify_event(event_id, 'speech_recognized', subscribed_id, "")
            elif methodname == 'person_localized':
                timestamp, person_ref, position_data = params
                try:
                    subscribed_id = self.event_ref_subscribe_id_table['person_localized']
                except KeyError:
                    #  print("This is un-subscribed event. (person_localized)")
                    continue
                self.event_id_result_table[event_id] = params
                self.notify_event(event_id, 'person_localized', subscribed_id, "")
            else:
                raise RuntimeError("[main_engine]Unknown method-name of event.")
            time.sleep(3)

    def get_command_result(self, command_id, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        results = self.command_id_result_table[command_id]
        return (status, results)

    def subscribe(self, event_type, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        self.subscribe_id += 1
        subscribe_id = str(self.subscribe_id)
        if event_type == 'speech_recognized':
            condition += self.sr_robot
        self.event_ref_subscribe_id_table[event_type+condition] = subscribe_id
        return (status, subscribe_id)

    def unsubscribe(self, subscribe_id):
        status = RoIS_HRI.ReturnCode_t.OK.value
        for key, value in self.event_ref_subscribe_id_table.items():
            if value == subscribe_id: event_ref = key
        del self.event_ref_subscribe_id_table[event_ref]
        return status

    def get_event_detail(self, event_id, condition):
        status = RoIS_HRI.ReturnCode_t.OK.value
        results = self.event_id_result_table[event_id]
        return (status, results)


class IF_server:
    """IF_Server
    """
    def __init__(self, port):
        self._addr = os.getenv("HRIENGINE")
        self._server = ThreadingXMLRPCServer(
            ("0.0.0.0", port), logRequests=False)

    def run(self, _IF):
        """IF_Server
        """
        self._server.register_instance(_IF)
        self._server.register_introspection_functions()
        # print("server running")
        self._server.serve_forever()


class MyHRIE:
    """IF_Server
    """
    def __init__(self):
        self.state = False


def test_engine(port):
    """test_engine
    """
    IF_server(port).run(IF(MyHRIE()))


if __name__ == "__main__":
    test_engine(8000)
