#!/usr/bin/env python

# The Joystick Controller Node for the tutorial "Up and flying with the AR.Drone and ROS | Joystick Control"
# https://github.com/mikehamer/ardrone_tutorials

# This controller implements the base DroneVideoDisplay class, the DroneController class and subscribes to joystick messages

# 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

# Load the DroneController class, which handles interactions with the drone, and the DroneVideoDisplay class, which handles video display
from drone_controller import BasicDroneController
from drone_video_display import DroneVideoDisplay

# Import the joystick message
from sensor_msgs.msg import Joy

# Finally the GUI libraries
from PySide import QtCore, QtGui

# define the default mapping between joystick buttons and their corresponding actions
ButtonEmergency = 0
ButtonLand      = 1
ButtonTakeoff   = 2

# define the default mapping between joystick axes and their corresponding directions
AxisRoll        = 0
AxisPitch       = 1
AxisYaw         = 3
AxisZ           = 4

AxisZDown		= 2
AxisZUp			= 5
YawLeft			= 11
YawRight 		= 12

# define the default scaling to apply to the axis inputs. useful where an axis is inverted
ScaleRoll       = 1.0
ScalePitch      = 1.0
ScaleYaw        = 1.0
ScaleZ          = 1.0

# special custom actions
TurnAround		= 3
StopTurn		= 4
Rotating 		= 0
ChangeScale		= 2

# handles the reception of joystick packets
def ReceiveJoystickMessage(data):
	global Rotating
	global ScaleRoll
	global ScalePitch, ScaleYaw, ScaleZ
	if data.buttons[ButtonEmergency]==1:
		rospy.loginfo("Emergency Button Pressed")
		controller.SendEmergency()
	elif data.buttons[ButtonLand]==1:
		rospy.loginfo("Land Button Pressed")
		controller.SendLand()
	elif data.buttons[ButtonTakeoff]==1:
		rospy.loginfo("Takeoff Button Pressed")
		controller.SendTakeoff()
	elif data.buttons[TurnAround]==1:
		rospy.loginfo("Engage autonomous room scanning")
		controller.StartRotating()
	elif data.buttons[ChangeScale]==1:
		if ScaleRoll == 1.0:
			ScaleRoll = 2.0
			ScalePitch = 2.0
			ScaleYaw = 2.0
			ScaleZ = 2.0
		else:
			ScaleRoll = 1.0
			ScalePitch = 1.0
			ScaleYaw = 1.0
			ScaleZ = 1.0
		rospy.loginfo("Current Scale Set To " + str(ScaleRoll))
	else:
		controller.SetCommand(data.axes[AxisRoll]/ScaleRoll,data.axes[AxisPitch]/ScalePitch,data.buttons[YawLeft]/ScaleYaw,data.buttons[YawRight]/ScaleYaw,data.axes[AxisZUp],data.axes[AxisZDown])


# Setup the application
if __name__=='__main__':
	import sys
	# Firstly we setup a ros node, so that we can communicate with the other packages
	rospy.init_node('ardrone_joystick_controller')

	# Next load in the parameters from the launch-file
	ButtonEmergency = int (   rospy.get_param("~ButtonEmergency",ButtonEmergency) )
	ButtonLand      = int (   rospy.get_param("~ButtonLand",ButtonLand) )
	ButtonTakeoff   = int (   rospy.get_param("~ButtonTakeoff",ButtonTakeoff) )
	AxisRoll        = int (   rospy.get_param("~AxisRoll",AxisRoll) )
	AxisPitch       = int (   rospy.get_param("~AxisPitch",AxisPitch) )
	AxisYaw         = int (   rospy.get_param("~AxisYaw",AxisYaw) )
	AxisZ           = int (   rospy.get_param("~AxisZ",AxisZ) )
	ScaleRoll       = float ( rospy.get_param("~ScaleRoll",ScaleRoll) )
	ScalePitch      = float ( rospy.get_param("~ScalePitch",ScalePitch) )
	ScaleYaw        = float ( rospy.get_param("~ScaleYaw",ScaleYaw) )
	ScaleZ          = float ( rospy.get_param("~ScaleZ",ScaleZ) )

	AxisZDown		= int ( rospy.get_param("~AxisZDown",AxisZDown) )
	AxisZUp			= int ( rospy.get_param("~AxisZUp",AxisZUp) )
	
	YawLeft		= int ( rospy.get_param("~YawLeft",YawLeft) )
	YawRight	= int ( rospy.get_param("~YawRight",YawRight) )
	
	TurnAround		= int	( rospy.get_param("~TurnAround", TurnAround) )
	ChangeScale		= int   (rospy.get_param("~ChangeScale", ChangeScale) )
	Rotating 		= 0
	# Now we construct our Qt Application and associated controllers and windows
	app = QtGui.QApplication(sys.argv)
	display = DroneVideoDisplay()
	controller = BasicDroneController()

	# subscribe to the /joy topic and handle messages of type Joy with the function ReceiveJoystickMessage
	subJoystick = rospy.Subscriber('/joy', Joy, ReceiveJoystickMessage)
	
	# executes the QT application
	display.show()
	status = app.exec_()

	# and only progresses to here once the application has been shutdown
	rospy.signal_shutdown('Great Flying!')
	sys.exit(status)
