#!/usr/bin/env python2
# coding:utf-8
import rospy

from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import MultiArrayLayout
import sys
from python import sim
from python.sim import *
from policy.orca import ORCA
import numpy as np
import time
# TODO:
from utils.human import Human
from utils.robot import Robot
import logging
import argparse
import configparser
from numpy.linalg import norm

actCmd = list()

class Interpreter():
	def __init__(self, config, robot):
		self.robot = robot
		self.config = config
		# self.clientID = clientID
		sub = rospy.Subscriber('chatter', Float32MultiArray, self.callback)
		self.pub = rospy.Publisher('act_cmd', Float32MultiArray, queue_size=2)
		# self.policy = ORCA()
		# TODO: initialize RVO2-Vrep sim
		self.time_step = 0.25

		# set Robot
		self.circle_radius = config.getfloat('sim', 'circle_radius')
		self.discomfort_dist = config.getfloat('reward', 'discomfort_dist')
		self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0, np.pi / 2)
		np.random.seed(1)
		self.generate_random_human_position(human_num=5)
		for agent in [self.robot] + self.humans:
			agent.time_step = self.time_step
			agent.policy.time_step = self.time_step

	# 把后边可能用到的 sub, pub 在初始化函数中定义好

	def callback(self, data):
		if (len(data.data)!=0):
			rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
			envState = list(data.data)
			# TODO: procedure to transfer envState to actCmd, just the decision-making procedure
			# prepare state info
			retFloats = envState
			for index, human in enumerate(self.humans):
				human.set_position([retFloats[4 * (index + 1) + 0], retFloats[4 * (index + 1) + 1]])
				human.set_velocity([retFloats[4 * (index + 1) + 2], retFloats[4 * (index + 1) + 3]])
				pass

			self.robot.set_position([retFloats[0], retFloats[1]])
			self.robot.set_velocity([retFloats[2], retFloats[3]])
			ob = [human.get_observable_state() for human in self.humans]

			action = self.robot.act(ob)
			# get act for each agent, generate actCmd
			human_actions = []
			for human in self.humans:
				# observation for humans is always coordinates
				ob = [other_human.get_observable_state() for other_human in self.humans if other_human != human]
				human_actions.append(human.act(ob))
			theta = np.arctan2(action.vy, action.vx)
			velocity = np.hypot(action.vx, action.vy)
			all_actions = [velocity, theta]
			for human_action in human_actions:
				theta = np.arctan2(human_action.vy, human_action.vx)
				velocity = np.hypot(human_action.vx, human_action.vy)
				all_actions.extend([velocity, theta])
				pass
			actCmd = all_actions #[1.0 for i in range(12)]
			data = Float32MultiArray()
			# retFloats = [round(i,4) for i in retFloats]
			data.data = actCmd
			# data.layout = MultiArrayLayout()
			self.pub.publish(data)

	def generate_random_human_position(self, human_num):
		self.humans = []
		for i in range(human_num):
			self.humans.append(self.generate_circle_crossing_human())

	def generate_circle_crossing_human(self):
		human = Human(self.config, 'humans')
		while True:
			angle = np.random.random() * np.pi * 2
			# add some noise to simulate all the possible cases robot could meet with human
			px_noise = (np.random.random() - 0.5) * human.v_pref
			py_noise = (np.random.random() - 0.5) * human.v_pref
			px = self.circle_radius * np.cos(angle) + px_noise
			py = self.circle_radius * np.sin(angle) + py_noise
			collide = False
			for agent in [self.robot] + self.humans:
				min_dist = human.radius + agent.radius + self.discomfort_dist
				if norm((px - agent.px, py - agent.py)) < min_dist or \
						norm((px - agent.gx, py - agent.gy)) < min_dist:
					collide = True
					break
			if not collide:
				break
		human.set(px, py, -px, -py, 0, 0, 0)
		return human

if __name__ == '__main__':
	print('Vrep Sim Program started 2')
	# configure environment
	parser = argparse.ArgumentParser('Parse configuration file')
	parser.add_argument('--env_config', type=str, default='env.config')
	args = parser.parse_args()
	env_config = configparser.RawConfigParser()
	env_config.read(args.env_config)

	robot = Robot(env_config, 'robot')

	rospy.init_node('interpreter')
	try:
		Interpreter(env_config, robot)
	except rospy.ROSInterruptException:
		pass
	# rate = rospy.Rate(250)
	# while not rospy.is_shutdown():
	#     rate.sleep()
	rospy.spin()