#!/usr/bin/env python

# A basic video display window for the tutorial "Up and flying with the AR.Drone and ROS | Getting Started"
# https://github.com/mikehamer/ardrone_tutorials_getting_started

# This display window listens to the drone's video feeds and updates the display at regular intervals
# It also tracks the drone's status and any connection problems, displaying them in the window's status bar
# By default it includes no control functionality. The class can be extended to implement key or mouse listeners if required

# Import the ROS libraries, and load the manifest file which through <depend package=... /> will give us access to the project dependencies
import roslib; roslib.load_manifest('ardrone_tutorials')
import rospy

# Import the two types of messages we're interested in
from sensor_msgs.msg import Image    	 # for receiving the video feed
from ardrone_autonomy.msg import Navdata # for receiving navdata feedback

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

# An enumeration of Drone Statuses
from drone_status import DroneStatus

# Import some stuff needed for OpenCV
import numpy as np
import cv2
fontFace = cv2.FONT_HERSHEY_DUPLEX
fontScale = 0.5

# The GUI libraries
from PySide import QtCore, QtGui


# Some Constants
CONNECTION_CHECK_PERIOD = 250 #ms
GUI_UPDATE_PERIOD = 20 #ms


class DroneVideoDisplay(QtGui.QMainWindow):
    StatusMessages = {
        DroneStatus.Emergency : 'Emergency',
        DroneStatus.Inited    : 'Initialized',
        DroneStatus.Landed    : 'Landed',
        DroneStatus.Flying    : 'Flying',
        DroneStatus.Hovering  : 'Hovering',
        DroneStatus.Test      : 'Test (?)',
        DroneStatus.TakingOff : 'Taking Off',
        DroneStatus.GotoHover : 'Going to Hover Mode',
        DroneStatus.Landing   : 'Landing',
        DroneStatus.Looping   : 'Looping (?)'
        }
    DisconnectedMessage = 'Disconnected'
    UnknownMessage = 'Unknown Status'
    
    def __init__(self):
        # Construct the parent class
        super(DroneVideoDisplay, self).__init__()

        # Setup our very basic GUI - a label which fills the whole window and holds our image
        self.setWindowTitle('AR.Drone Video Feed')
        self.imageBox = QtGui.QLabel(self)
        self.setCentralWidget(self.imageBox)

        # Subscribe to the /ardrone/navdata topic, of message type navdata, and call self.ReceiveNavdata when a message is received
        self.subNavdata = rospy.Subscriber('/ardrone/navdata',Navdata,self.ReceiveNavdata) 
        
        # Subscribe to the drone's video feed, calling self.ReceiveImage when a new frame is received
        #self.subVideo   = rospy.Subscriber('/ardrone/image_raw',Image,self.ReceiveImage)
        self.subVideoFront   = rospy.Subscriber('/ardrone/front/image_raw',Image,self.ReceiveImageFront)
        self.subVideoBottom   = rospy.Subscriber('/ardrone/bottom/image_raw',Image,self.ReceiveImageBottom)
        
        # Holds the image frame received from the drone and later processed by the GUI
        self.image = None
        self.frame_id = -1
        self.imageLock = Lock()
        
        # Holds the status message to be displayed on the next GUI update
        self.statusMessage = ''

        # Tracks whether we have received data since the last connection check
        # This works because data comes in at 50Hz but we're checking for a connection at 4Hz
        self.communicationSinceTimer = False
        self.connected = False

        # A timer to check whether we're 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
                self.frame_id = self.image.header.frame_id    # ardrone_base_frontcam/ardrone_base_bottomcam
                image = QtGui.QPixmap.fromImage(QtGui.QImage(self.image.data, self.image.width, self.image.height, QtGui.QImage.Format_RGB888))
                #img = np.array(buffer(self.image.data), dtype=np.uint8).reshape(360, 640, 3)
                #cv2.putText(img, self.frame_id, (180, 320), fontFace, fontScale, cv2.cv.CV_RGB(255,0,0), thickness=2)
                #image = QtGui.QPixmap.fromImage(QtGui.QImage(img, 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 + (" frame_id: %s" % self.frame_id)) 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()
    
    def ReceiveImageFront(self,data):
        data.header.frame_id = "FRONT"
        self.ReceiveImage(data)
    
    def ReceiveImageBottom(self,data):
        data.header.frame_id = "BOTTOM"
        self.ReceiveImage(data)

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

        # Update the message to be displayed
        msg = self.StatusMessages[navdata.state] if navdata.state in self.StatusMessages else self.UnknownMessage
        self.statusMessage = '{} (Battery: {}%)'.format(msg,int(navdata.batteryPercent))

if __name__=='__main__':
    import sys
    rospy.init_node('ardrone_video_display')
    app = QtGui.QApplication(sys.argv)
    display = DroneVideoDisplay()
    display.show()
    status = app.exec_()
    rospy.signal_shutdown('Great Flying!')
    sys.exit(status)
