#!/usr/bin/env python2
import rospy

from std_msgs.msg import String
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import MultiArrayLayout
import sys
sys.path.append ('./python')
import sim
from sim import *
import numpy as np
from utils.human import Human
from utils.robot import Robot
import logging
import argparse
import configparser
from numpy.linalg import norm
import time

class Sensor(object):
	"""docstring for ."""

	def __init__(self,clientID, config,robot):
		self.robot = robot
		self.config = config
		self.clientID = clientID
		self.all_actions = [0 for i in range(12)]
		sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
		time.sleep(0.5)
		sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
		time.sleep(0.5)
		sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot)
		time.sleep(0.5)

		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)

		# get current observation
		ob = [human.get_observable_state() for human in self.humans]

		# collect agents' states in gym env
		fullstate = self.robot.get_full_state()
		reset_info = []
		reset_info.extend([fullstate.px, fullstate.py])
		for index, human in enumerate(ob):
			# human_theta = np.arctan2(human.vy, human.vx)
			reset_info.extend([human.px, human.py])

		res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(self.clientID,
																					'remoteApiCommandServer',
																					sim.sim_scripttype_childscript,
																					'setRobotPos', [], reset_info,
																					[], '',
																					sim.simx_opmode_blocking)

		sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
		time.sleep(2)

		rospy.init_node('talker')
		self.pub = rospy.Publisher('chatter', Float32MultiArray, queue_size=2)
		self.sub = rospy.Subscriber('act_cmd', Float32MultiArray, self. callback)

		res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setVelGetPos', [],self.all_actions, [], '',sim.simx_opmode_blocking)
		rospy.loginfo(retStrings)
		data = Float32MultiArray()
		# retFloats = [round(i,4) for i in retFloats]
		data.data = retFloats
		# data.layout = MultiArrayLayout()
		self.pub.publish(data)
		time.sleep(0.1)
		self.pub.publish(data)
		time.sleep(0.1)
		self.pub.publish(data)
		time.sleep(0.1)
		self.pub.publish(data)

		rate = rospy.Rate(500)

		while not rospy.is_shutdown():
			# all_actions = [0 for i in range(12)]
			rate.sleep()


	def callback(self, data):
		if (len(data.data)!=0):
			rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
			actCmd = list(data.data)
			self.all_actions = actCmd
			# print(actCmd)
			res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setVelGetPos', [],self.all_actions, [], '',sim.simx_opmode_blocking)
			rospy.loginfo(retStrings)
			data = Float32MultiArray()
			# retFloats = [round(i,4) for i in retFloats]
			data.data = retFloats
			# 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')

	sim.simxFinish(-1)  # just in case, close all opened connections
	clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5)  # Connect to CoppeliaSim
	if clientID != -1:
		print('Connected to remote API server 2')
		# Now try to retrieve data in a blocking fashion (i.e. a service call):
		res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking)
		if res == sim.simx_return_ok:
			print('Number of objects in the scene 2: ', len(objs))
		else:
			print('Remote API function call returned with error code: ', res)
		# time.sleep(2)
	else:
		print('Failed connecting to remote API server')
		pass



	try:
		Sensor(clientID, env_config, robot)
	except rospy.ROSInterruptException:
		pass