# from pyrois.Service_Application_IF import Service_Application_IF
from pyrois import RoIS_Service, RoIS_HRI
from pyrois.RoIS_Common import Component_Status
from pyrois.RoIS_Service import Completed_Status
import HRI_Engine_client_sample
import command_message

import logging
import queue
import threading
import time
import enum
# import xmlrpc
from termcolor import colored
from transitions import Machine
import sys


class Task(enum.Enum):
    Init        = 'Init'
    GetUserPos  = 'GetUserPos'
    NaviToUser  = 'NaviToUser'
    ListenDest  = 'ListenDest'
    NaviToGuide = 'NaviToGuide'
    End         = 'End'


class Service(HRI_Engine_client_sample.IF):
    def __init__(self, uri="http://127.0.0.1:8000", logger=None):
        HRI_Engine_client_sample.IF.__init__(self, uri, logger=None, event_queue=None)
        # buid state machine
        states = [Task.Init, Task.GetUserPos, Task.NaviToUser, Task.ListenDest, Task.NaviToGuide, Task.End]
        transitions = [
            {'trigger': 'Start',       'source': Task.Init,        'dest': Task.GetUserPos,  'after': 'get_user_position'  },
            {'trigger': 'PLSuccess',   'source': Task.GetUserPos,  'dest': Task.NaviToUser,  'after': 'start_navi_to_user' },
            {'trigger': 'NaviSuccess', 'source': Task.NaviToUser,  'dest': Task.ListenDest,  'after': 'listen_destination' },
            {'trigger': 'SRSuccess',   'source': Task.ListenDest,  'dest': Task.NaviToGuide, 'after': 'start_navi_to_guide'},
            {'trigger': 'InvalidDest', 'source': Task.NaviToGuide, 'dest': Task.ListenDest,  'after': 'listen_destination' },
            {'trigger': 'NaviFail',    'source': Task.NaviToUser,  'dest': Task.NaviToUser,  'after': 'start_navi_to_user' },
            {'trigger': 'NaviFail',    'source': Task.NaviToGuide, 'dest': Task.End,         'after': 'abort_service'      },
            {'trigger': 'NaviSuccess', 'source': Task.NaviToGuide, 'dest': Task.End,         'after': 'complete_service'   },
            {'trigger': 'NoRoboAvail', 'source': Task.NaviToUser,  'dest': Task.End,         'after': 'abort_service'      },
        ]
        self.machine = Machine(self,
                               states=states,
                               transitions=transitions,
                               initial=Task.Init,
                               ignore_invalid_triggers=True)
        self.latest_event_id_table = {}
        self.logger.info("service_app_with_eng_client.Service(): complete init")

    def completed(self, command_id, completed_status):
        completed_status = Completed_Status(completed_status)
        if completed_status == Completed_Status.OK:
            print(colored("robot reached goal successfully.", 'green'))
            self.NaviSuccess()
        elif completed_status == Completed_Status.ABORT:
            print(colored("robot failed to reach goal.", 'red'))
            self.NaviFail()

    def notify_error(self, error_id, error_type):
        print('received error event {}({}) '.format( \
              error_id, RoIS_Service.ErrorType(error_type).name))

    def notify_event(self, event_id, event_type, subscribe_id, expire):
        if event_type == 'speech_recognized':
            self.latest_event_id_table['speech_recognized'] = event_id
            return_code = self.unsubscribe(subscribe_id)
            self.SRSuccess()
        if event_type == 'person_localized':
            self.latest_event_id_table['person_localized'] = event_id
            self.PLSuccess()

    def get_user_position(self):
        return_code, subscribe_id = self.subscribe('person_localized', "")
        print(colored("GetUserPos started.", 'cyan'))

    def start_navi_to_user(self):
        pl_event_id = self.latest_event_id_table['person_localized']
        status, results = self.get_event_detail(pl_event_id, "")
        print(colored("person localized successfully.", 'green'))
        dest = results[3]
        print("Destination is {} ({})".format(dest, type(dest).__name__))
        # (return_code, command_id) = self.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""])
        c_ri = command_message.RoIS_Identifier("","")
        c_p_n1 = command_message.Parameter("target_position", c_ri, dest)
        c_p_n2 = command_message.Parameter("time_limit", c_ri, "")
        c_p_n3 = command_message.Parameter("routing_policy", c_ri, "")
        c_al = command_message.ArgumentList([c_p_n1,c_p_n2,c_p_n3])
        c_cm = command_message.CommandMessage('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation',"set_parameter","",c_al)
        c_cm.delay_time = 0
        c_cus = command_message.CommandUnitSequence([c_cm])
        (return_code, command_unit_list) = self.execute(c_cus)
        status = RoIS_HRI.ReturnCode_t(return_code)
        # if status == RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES:
        if status != RoIS_HRI.ReturnCode_t.OK:
            print(colored("There is no robot that is available.", 'red'))
            self.NoRoboAvail()
        print(colored("NaviToUser started.", 'cyan'))

    def listen_destination(self):
        return_code, subscribe_id = self.subscribe('speech_recognized', "")
        print(colored("ListenDest started.", 'cyan'))

    def start_navi_to_guide(self):
        sr_event_id = self.latest_event_id_table['speech_recognized']
        status, results = self.get_event_detail(sr_event_id, "")
        print(colored("speech recognized successfully.", 'green'))
        dest = results[2][0]
        print("Destination is {} ({})".format(dest, type(dest).__name__))
        # (return_code, command_id) = self.set_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation', [dest, "", ""])
        c_ri = command_message.RoIS_Identifier("","")
        c_p_n1 = command_message.Parameter("target_position", c_ri, dest)
        c_p_n2 = command_message.Parameter("time_limit", c_ri, "")
        c_p_n3 = command_message.Parameter("routing_policy", c_ri, "")
        c_al = command_message.ArgumentList([c_p_n1,c_p_n2,c_p_n3])
        c_cm = command_message.CommandMessage('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation',"set_parameter","",c_al)
        c_cm.delay_time = 0
        c_cus = command_message.CommandUnitSequence([c_cm])
        (return_code, command_unit_list) = self.execute(c_cus)
        status = RoIS_HRI.ReturnCode_t(return_code)
        # if status == RoIS_HRI.ReturnCode_t.BAD_PARAMETER:
        if status != RoIS_HRI.ReturnCode_t.OK:
            print(colored("Bad destination detected.", 'yellow'))
            print(colored("Trying to ask again...", 'yellow'))
            self.InvalidDest()
        else:
            print(colored("NaviToGuide started.", 'cyan'))

    def complete_service(self):
        print(colored("Route Guidance Completed !", 'cyan'))
        sys.exit(0)

    def abort_service(self):
        print(colored("Route Guidance Aborted...", 'red'))
        sys.exit(1)

    def start_service(self):
        self.connect()
        
        self.get_profile("")
        self.search("")
        self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:Navigation')
        self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation')
        self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:SpeechRecognition')
        self.bind('urn:x-rois:def:HRIComponent:Kyutech:main:PersonLocalization')

        c_ri = command_message.RoIS_Identifier("","")
        c_p_si = command_message.Parameter("", c_ri, "")
        c_al = command_message.ArgumentList([c_p_si])
        c_cm = command_message.CommandMessage('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation',"get_parameter","",c_al)
        c_cm.delay_time = 0
        c_cus = command_message.CommandUnitSequence([c_cm])
        self.execute(c_cus)
        # self.get_parameter('urn:x-rois:def:HRIComponent:Kyutech:main:SystemInformation')

        self.Start()
        print(self.state)
        # self.PLSuccess()
        # print(self.state)


if __name__ == '__main__':
    service = Service()
    print("Starting service..")
    service.start_service()
  

    # print(Service.mro())
    """
    [<class '__main__.Service'>, <class 'HRI_Engine_client_sample.IF'>, <class 'HRI_Engine_client_sample.SystemIF'>, 
    <class 'pyrois.RoIS_HRI.SystemIF'>, <class 'HRI_Engine_client_sample.CommandIF'>, <class 'pyrois.RoIS_HRI.CommandIF'>, 
    <class 'HRI_Engine_client_sample.QueryIF'>, <class 'pyrois.RoIS_HRI.QueryIF'>, <class 'HRI_Engine_client_sample.EventIF'>, 
    <class 'pyrois.RoIS_HRI.EventIF'>, <class 'Service_Application_IF_sample.Service_Application_IF'>, <class 'pyrois.RoIS_Service.Service_Application_Base'>, 
    <class 'object'>]
    """