#!/usr/bin/env python
# -*- coding: utf-8 -*-

# VirtualNavigation.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()

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

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


class Mode(enum.Enum):
    NaviToUser  = 'NaviToUser'
    NaviToGuide = 'NaviToGuide'


class VirtualNavigation(Event, Command, Query):
    def __init__(self, c, robot_names):
        super(VirtualNavigation, self).__init__(c)
        self._component = c
        self._component.Target_Position = [""]
        self._component.Time_Limit = 10
        self._component.Routing_Policy = ""
        self.init_pos_table = {
            'robot1': [0.0,  0.0,  0.0],
            'robot2': [10.0, 0.0,  0.0],
            'robot3': [0.0,  10.0, 0.0]
        }
        self.robot_goal_table = {}
        self.nav_state_table = \
            {robot_name : Component_Status.READY for robot_name in robot_names}
            #  {robot_name : Component_Status.UNINITIALIZED for robot_name in robot_names}
        self.pointname_coord_table = yaml.load(open("field_points.yaml"))
        self.mode = Mode.NaviToUser
        self.latest_target_robot = None

    def position_to_coord(self, position):
        if isinstance(position, str):
            return self.pointname_coord_table[position]
        else:
            return position

    def set_parameter(self, target_position, time_limit, routing_policy):
        if all([state != Component_Status.READY for state in self.nav_state_table.values()]):
            return RoIS_HRI.ReturnCode_t.OUT_OF_RESOURCES
        # ERRORなロボットは初期位置に戻す
        self.back_stucked_robots_to_init_pos()

        try:
            target_coord = self.position_to_coord(target_position)
        except KeyError:
            return RoIS_HRI.ReturnCode_t.BAD_PARAMETER.value
        if self.mode == Mode.NaviToUser:
            # READYなロボットの中でゴールに最も近い位置にいるものにNavigationさせる
            target_robot = self._component.get_nearest_robotname(target_coord, self.nav_state_table)
        else:  # self.mode == Mode.NaviToGuide:
            # ユーザのもとに辿り着いたロボットに Navigation させる
            target_robot = self.latest_target_robot

        th = threading.Thread(
            target=self.send_goal_and_event,
            args=(target_position, target_robot))
        th.daemon = True
        th.start()
        status = RoIS_HRI.ReturnCode_t.OK.value
        return status

    def send_goal_and_event(self, goal, robot_name):
        goal_coord = self.position_to_coord(goal)
        self.nav_state_table[robot_name] = Component_Status.BUSY
        status = self._component.send_goal_to_robot_async(goal_coord, robot_name)
        self.nav_state_table[robot_name] = status
        if status == Component_Status.READY: # Navigation succeeded
            self.mode = Mode.NaviToGuide if self.mode == Mode.NaviToUser else Mode.NaviToUser
        self.latest_target_robot = robot_name
        msg = xmlrpclib.dumps((goal, robot_name, status.value), 'nav_finished')
        self.event_queue.put(msg)

    def back_stucked_robots_to_init_pos(self):
        def reset_robot_pos_and_state(robot_name):
            init_pos = self.init_pos_table[robot_name]
            init_pos = self.position_to_coord(init_pos)
            self._component.send_goal_to_robot_async(init_pos, robot_name)
            self.nav_state_table[robot_name] = Component_Status.READY

        for robot_name, state in self.nav_state_table.items():
            if state == Component_Status.ERROR:
                th = threading.Thread(
                        target=reset_robot_pos_and_state,
                        args=(robot_name, ))
                th.daemon = True
                th.start()


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

import numpy as np
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped

from functools import partial


class Component(object):
    def __init__(self, robot_names):
        self.__robot_names = robot_names
        self.__goal_senders = \
            {robot_name : GoalSenderROS(robot_name) for robot_name in robot_names}
        self.__robot_positions = {robot_name : None for robot_name in robot_names}
        for robot_name in robot_names:
            rospy.Subscriber(robot_name+"/amcl_pose", 
                             PoseWithCovarianceStamped,
                             partial(self.update_position, robot_name))

    def update_position(self, robot_name, amcl_pose):
        pos = amcl_pose.pose.pose.position
        self.__robot_positions[robot_name] = np.array([pos.x, pos.y, pos.z])

    def send_goal_to_nearest_robot(self, dest, state_table):
        dest = np.array(dest)
        distance_table = {}
        for name, pos in self.__robot_positions.items():
            distance_table[name] = np.linalg.norm(dest - pos) \
                if state_table[name] == Component_Status.READY else float('inf')
        nearest_robot = min(distance_table, key=distance_table.get)
        self.__goal_senders[nearest_robot].send_goal(dest.tolist())
        return nearest_robot

    def get_nearest_robotname(self, dest, state_table):
        dest = np.array(dest)
        distance_table = {}
        for name, pos in self.__robot_positions.items():
            distance_table[name] = np.linalg.norm(dest - pos) \
                if state_table[name] == Component_Status.READY else float('inf')
        nearest_robot = min(distance_table, key=distance_table.get)
        return nearest_robot

    def send_goal_to_robot(self, goal, robot):
        self.__goal_senders[robot].send_goal(goal)

    def get_nav_state(self, robot_name):
        goal_sender = self.__goal_senders[robot_name]
        return goal_sender.get_goalsender_state()

    def send_goal_to_robot_async(self, goal, robot):
        return self.__goal_senders[robot].send_goal_and_wait(goal)


def example_n(port, robot_names):
    print("Starting node...")
    component = Component(robot_names)
    n = VirtualNavigation(component, robot_names)
    print("VirtualNavigation constructed")

    # 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> VirtualNavigation.py <port> <robot_name1> <robot_name2> ...")
        raise RuntimeError

    port, robot_names = int(sys.argv[1]), sys.argv[2:]
    rospy.init_node('virtual_navigation_component', anonymous=True)
    example_n(port=port, robot_names=robot_names)
