#!/usr/bin/env python3

# Person_Localization.py

"""Person_Localization
"""

import sys
from six.moves import queue
import time
from datetime import datetime
import threading
from six.moves import xmlrpc_client

from pyrois import RoIS_Common, RoIS_HRI

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

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

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


class Command(RoIS_Common.Command):
    """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

    def set_parameter(self, detection_threshold, minimum_interval):
        self._component.Detection_Threshold = detection_threshold
        self._component.Minimum_Interval = minimum_interval
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status


class Query(RoIS_Common.Query):
    """Query
    """
    def __init__(self, c):
        self._component = c
        # self._component.Detection_Threshold = 1000
        # self._component.Minimum_Interval = 20

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

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

    def poll_event(self):
        """poll_event
        """
        msg = self.event_queue.get()
        return msg

    def person_localized(self, timestamp, person_ref, position_data):
        """person_localization
        """
        msg = xmlrpc_client.dumps((timestamp, person_ref, position_data), 'person_localized')
        self.event_queue.put(msg)


import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

class Person_Localization(Event, Command, Query):
    """Person_Localization
    """
    def __init__(self, c, person_name):
        super(Person_Localization, self).__init__(c)
        self._component = c
        self._component.Detection_Threshold = 1000
        self._component.Minimum_Interval = 20
        self.person_ref = person_name
        rospy.Subscriber(person_name+'/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_callback)
        self.position = None

    def amcl_pose_callback(self, amcl_pose):
        pos = amcl_pose.pose.pose.position
        self.position = [pos.x, pos.y, 0.0]
        self.person_localized(datetime.now(), self.person_ref, self.position)


class component:
    """component
    """
    def __init__(self):
        self._state = False


def example_pl(port, person_name):
    """example_pl
    """
    c = component()
    pl = Person_Localization(c, person_name)

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


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

    port, person_name = int(sys.argv[1]), sys.argv[2]
    rospy.init_node('person_localization', anonymous=True)
    example_pl(port, person_name)
