#!/usr/bin/env python
import roslib
roslib.load_manifest("rpc_game_master")
import rospy
from sensor_msgs.msg import CompressedImage, CameraInfo
from std_msgs.msg import Header
from rpc_game_client.srv import PlayerScore


class TestPlayer:
	def __init__(self):
		rospy.on_shutdown(self.exit)
		rospy.init_node("test_player")
		self.interval = rospy.Duration(11)
		self.last_image = rospy.Time(0)
		self.camera_info = False
		self.camera_type = None
		rospy.loginfo("[TestPlayer] Waiting for scoring service")
		rospy.wait_for_service('/rpc_score')
		rospy.loginfo("[TestPlayer] Scoring service found")
		rospy.loginfo("[TestPlayer] Checking for Camera")
		while True:
			rospy.loginfo("[TestPlayer] Trying for Kinect")
			try:
				self.camera_info = rospy.wait_for_message("/camera/rgb/camera_info", CameraInfo, timeout=2)
				self.camera_type = "Kinect"
				break
			except rospy.exceptions.ROSException, e:
				pass
			rospy.loginfo("[TestPlayer] Trying for usb_cam")
			try:
				self.camera_info = rospy.wait_for_message("/usb_cam/camera_info", CameraInfo, timeout=2)
				self.camera_type = "usb_cam"
				break
			except rospy.exceptions.ROSException, e:
				pass
			rospy.logwarn("[TestPlayer] No CameraInfo found, trying again")
		rospy.sleep(0.2)
		if self.camera_type == "Kinect":
			rospy.Subscriber("/camera/rgb/image_color/compressed", CompressedImage, self.push_image, queue_size=1)
		if self.camera_type == "usb_cam":
			rospy.Subscriber("/usb_cam/image_raw/compressed", CompressedImage, self.push_image, queue_size=1)
		rospy.loginfo("[TestPlayer] Started TestPlayer using %s" % self.camera_type)
		rospy.spin()

	def push_image(self, data):
		if ((rospy.Time.now() - self.last_image) > self.interval):
			self.last_image = rospy.Time.now()
			try:
				score = rospy.ServiceProxy('/rpc_score', PlayerScore)
				header = Header()
				header.stamp = rospy.Time.now()
				response = score(data, self.camera_info)
				rospy.loginfo("[TestPlayer] Scored: %s (%s+%s+%s)" % (response.score.total, response.score.align, response.score.center, response.score.distance))
			except rospy.ServiceException, e:
				rospy.logerr("[TestPlayer] Service call failed: %s" % e)

	def exit(self):
		rospy.logerr("[TestPlayer] Shutdown requested!")

if __name__ == '__main__':
	try:
		test_player = TestPlayer()
	except rospy.ROSInterruptException:
		pass