#!/usr/bin/env python3

# Navigation.py
#
# Copyright 2018 Eiichi Inohira
# This software may be modified and distributed under the terms
# of the MIT license
#
# For python3
# For HRI Component

import sys
#  import queue
import time
from datetime import datetime
import threading

from pyrois import RoIS_Common, RoIS_HRI

if sys.version_info.major == 2:
    import Queue as queue
    import SocketServer
    import SimpleXMLRPCServer

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

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


class Command(RoIS_Common.Command):
    def __init__(self, c):
        self._component = c

    def start(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def stop(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def suspend(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def resume(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status


class Query(RoIS_Common.Query):
    def __init__(self, c):
        self._component = c

    def component_status(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        c_status = RoIS_Common.Component_Status.UNINITIALIZED.value
        return (status, c_status)


class Event(RoIS_Common.Event):
    def __init__(self, c):
        self._component = c
        self.event_queue = queue.Queue()


from goal_sender_ros import GoalSenderROS
# import xmlrpclib
import yaml
from pyrois.RoIS_Common import Component_Status

class Navigation(Event, Command, Query):
    def __init__(self, c, robot_name):
        super(Navigation, self).__init__(c)
        self._component = c
        self._goal_sender = GoalSenderROS(robot_name)
        self._component.Target_Position = [""]
        self._component.Time_Limit = 10
        self._component.Routing_Policy = ""
        with open("field_points.yaml","r") as f:
            self.pointname_coord_table = yaml.safe_load(f)

    def set_parameter(self, target_position, time_limit, routing_policy):
        current_status = self._goal_sender.get_goalsender_state()
        if current_status == RoIS_Common.Component_Status.ERROR:
            return RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES.value
        if isinstance(target_position, str):
            try:
                target_position = self.pointname_coord_table[target_position]
            except KeyError:
                return RoIS_HRI.ReturnCode_t.BAD_PARAMETER.value
        self._goal_sender.send_goal(target_position)
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def component_status(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        c_status = self._goal_sender.get_goalsender_state().value
        return (status, c_status)


def event_dispatch(n):
    #  n.person_detected(datetime.now().isoformat(), 1)
    time.sleep(0.1)
    #  n.person_detected(datetime.now().isoformat(), 1)


class Component(object):
    pass

import rospy

def example_n(port, robot_name):
    print("Starting node...")
    component = Component()
    n = Navigation(component, robot_name)
    print(f"Navigation constructed {robot_name}")

    # start the timer to dispatch events
    t = threading.Timer(0.1, event_dispatch, args=(n,))
    t.start()

    # start the XML-RPC server
    server = ThreadingXMLRPCServer(("0.0.0.0", port), logRequests=False)
    server.register_instance(n)
    server.register_introspection_functions()
    server.register_multicall_functions()
    server.serve_forever()


if __name__ == '__main__':
    if len(sys.argv) < 3:
        print("rosrun <this_package> Navigation.py <port_num> <robot_name>")
        raise RuntimeError

    port, robot_name = int(sys.argv[1]), sys.argv[2]
    rospy.init_node('navigation_component', anonymous=True)
    example_n(port=port, robot_name=robot_name)
