#!/usr/bin/env python

import rospy

from sensor_msgs.msg import Image		# for recieving video feed
from geometry_msgs.msg import Twist		# controlling the movements
from diagnostic_msgs.msg import DiagnosticArray 



# Use resource locking to handle synchronization between GUI thread and ROS topic callbacks
from threading import Lock

# Import Python GUI Libraries
from PySide import QtCore, QtGui


# CONSTANTS
CONNECTION_CHECK_PERIOD = 250 #ms
GUI_UPDATE_PERIOD = 20 #ms


class TurtlebotImageView(QtGui.QMainWindow):
	""" Class doc """
	# you can define a call back function here to print status on the
	# Image window
	
	
	DisconnectedMessage = 'Disconnected'
	UnknownMessage = 'Unknown Status'
	def __init__ (self):
		""" Class initialiser """
		# constructor
		
		super(TurtlebotImageView, self).__init__()
		
		
		#basic GUI Setup
		self.setWindowTitle('Turtlebot Video Feed')
		self.imageBox = QtGui.QLabel(self)
		self.setCentralWidget(self.imageBox)
		
		
		# Subscribe to the turtlebots video 
		self.subVideo = rospy.Subscriber('/camera/rgb/image_raw', Image, self.ReceiveImage)
		
		
		# Hold the image recieved to be processed by the GUI
		self.image = None
		self.imageLock = Lock();
		
		
		# Timers to keep track of when data was last recieved
		self.communicationSinceTimer = False
		self.connected = False
		
		
		# timer to check if still connected
		self.connectionTimer = QtCore.QTimer(self)
		self.connectionTimer.timeout.connect(self.ConnectionCallback)
		self.connectionTimer.start(CONNECTION_CHECK_PERIOD)
		
		# A timer to redraw the GUI
		self.redrawTimer = QtCore.QTimer(self)
		self.redrawTimer.timeout.connect(self.RedrawCallback)
		self.redrawTimer.start(GUI_UPDATE_PERIOD)
		
		
		# Called every CONNECTION_CHECK_PERIOD ms, if we haven't received anything since the last callback, will assume we are having network troubles and display a message in the status bar
	def ConnectionCallback(self):
		self.connected = self.communicationSinceTimer
		self.communicationSinceTimer = False

	def RedrawCallback(self):
		if self.image is not None:
			# We have some issues with locking between the display thread and the ros messaging thread due to the size of the image, so we need to lock the resources
			self.imageLock.acquire()
			try:			
					# Convert the ROS image into a QImage which we can display
					image = QtGui.QPixmap.fromImage(QtGui.QImage(self.image.data, self.image.width, self.image.height, QtGui.QImage.Format_RGB888))
					
			finally:
				self.imageLock.release()

			# We could  do more processing (eg OpenCV) here if we wanted to, but for now lets just display the window.
			self.resize(image.width(),image.height())
			self.imageBox.setPixmap(image)

		# Update the status bar to show the current drone status & battery level
		self.statusBar().showMessage(self.statusMessage if self.connected else self.DisconnectedMessage)

	def ReceiveImage(self,data):
		# Indicate that new data has been received (thus we are connected)
		self.communicationSinceTimer = True

		# We have some issues with locking between the GUI update thread and the ROS messaging thread due to the size of the image, so we need to lock the resources
		self.imageLock.acquire()
		try:
			self.image = data # Save the ros image for processing by the display thread
		finally:
			self.imageLock.release()
		
		
if __name__=='__main__':
	import sys
	rospy.init_node("Turtlebot_ImageView")
	app = QtGui.QApplication(sys.argv)
	display = TurtlebotImageView()
	display.show()
	status= app.exec_()
	rospy.signal_shutdown("End")
	
	sys.exit(0)