#!/usr/bin/env python3

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

"""System_Information
"""

import sys
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):
        """ThreadingXMLRPCServer
        """
        pass


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

    def component_status(self):
        pass
    
    def robot_position(self):
        pass

    def engine_status(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        e_status = RoIS_Common.Component_Status.READY.value
        operable_time = ["2100-01-01T00:00:01+09:00","2100-01-01T00:00:01+09:00"]
        return (status, e_status, operable_time)


class System_Information(Query):
    """System_Information
    """
    def __init__(self, c, robot_name):
        super(System_Information, self).__init__(c)
        self._robot_name = robot_name
        self._component = c

    def robot_position(self):
        status = RoIS_HRI.ReturnCode_t.OK.value
        timestamp = str(datetime.now())
        robot_ref = ["urn:x-rois:def:HRIComponent:Kyutech::{}".format(self._robot_name)]
        position_data = self._component.get_position()
        return (status, timestamp, robot_ref, position_data)


import rospy
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped

class component:
    """component
    """
    def __init__(self, robot_name):
        self._state = False
        self.position = None
        rospy.Subscriber(robot_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback)

    def amcl_pose_callback(self, amcl_pose):
        pos = amcl_pose.pose.pose.position
        self.position = [pos.x, pos.y]

    def get_position(self):
        return self.position

def event_dispatch(pd):
    """event_dispatch
    """

def example_si(port, robot_name):
    """example_si
    """
    c = component(robot_name)
    si = System_Information(c, robot_name)

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

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


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

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