#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Twist
from sensor_msgs.msg import JointState
import os

class JoystickNode:
	def __init__(self):
		self.motors_stopped = False
		self.flipper_stopped = False
		self.dying = False
		self.max_speed = rospy.get_param('~speed', 1.0)
		self.max_turn = rospy.get_param('~turn', 0.75)
		self.joint_states = {}
		self.twistPub = rospy.Publisher("/base/cmd_vel", Twist, queue_size=1)
		self.armPub = rospy.Publisher("/joint_control", JointState, queue_size=1)
		rospy.Subscriber("joy", Joy, self.callback)
		rospy.Subscriber("joint_states", JointState, self.callbackJoints)


	def setVelocities(self, forward, turn):
		cmd = Twist()
		cmd.linear.x = forward * self.max_speed;
		cmd.linear.y = cmd.linear.z = 0;
		cmd.angular.z = turn * self.max_turn;
		cmd.angular.y = cmd.angular.x = 0;
		self.twistPub.publish(cmd)
	
	def callbackJoints(self, joint_data):
		for i in range(len(joint_data.name)):
			self.joint_states[joint_data.name[i]] = joint_data.position[i]

	def callback(self, data):
		if self.dying:
			return
#		rospy.loginfo(rospy.get_caller_id() + " axes %s", data.axes)
#		rospy.loginfo(rospy.get_caller_id() + " buttons %s", data.buttons)

		# Drive Motor Control
		if data.buttons[6] == 1:
			self.motors_stopped = False
			self.setVelocities((data.axes[1] + data.axes[3]) / 2, (data.axes[0] + data.axes[2]) / 2)
		elif not self.motors_stopped:
			self.setVelocities(0.0, 0.0)
			self.motors_stopped = True
		
		# Kill ROS launch file
		if (data.buttons[1] == 1 and data.buttons[7] == 1 and data.buttons[8] == 1 and data.buttons[9] == 1):
			# wireless D/off -> A, RT, back, start
			self.dying = True
			self.setVelocities(0,0)
			rospy.logwarn(rospy.get_caller_id() + " Killing roslaunches.")
			os.system("killall -s SIGINT roslaunch")
			return

		
		# Shutdown the Computer
		if (data.buttons[6] == 0 and data.buttons[2] == 1 and data.buttons[4] == 1 and data.buttons[7] == 1 and data.buttons[9] == 1 and data.buttons[10] == 1):
			# wireless D/off -> B, LB, RT, start, left stick
			self.dying = True
			self.setVelocities(0,0)
			rospy.logwarn(rospy.get_caller_id() + " Shutting down the machine.")
			os.system("sudo shutdown -P now")
#			os.system("killall -s SIGINT roslaunch")
			return
		
		joints = []
		positions = []
		angle_change = 0.1
		
                # Zero joints
                if (data.buttons[7] == 1):
#                        joints = ["neck_tilt", "neck_pan"]
#                        positions = [ 0.0, 0.0 ]

                        if (data.axes[5] == 1):
                                joints = ["neck_tilt", "neck_pan", "arm_base", "arm_shoulder", "arm_elbow"]
                                positions = [ 0.0, 0.0, 0.0, 0.0, 0.0 ]
			elif (data.axes[5] == -1):
				joints = ["neck_tilt", "neck_pan"]
	                        positions = [ 0.0, 0.0 ]
			elif (data.axes[4] == -1):
				joints[:] = []
				positions[:] = []
				joints = ["neck_tilt", "neck_pan", "arm_base", "arm_shoulder", "arm_elbow"]
				positions = [ -0.785, 0.0, 0.0, -0.785, 1.57 ]	
			elif (data.axes[4] == 1):
                                joints[:] = []
                                positions[:] = []
                                joints = ["neck_tilt", "neck_pan", "arm_base", "arm_shoulder", "arm_elbow"]
                                positions = [ -1.25, 0.0, 0.0, -1.25, 2.5 ]
		else:
			# arm_base
			if (data.buttons[4] == 1 and data.buttons[5] == 1):
				# Do nothing they cancel out
				pass
			elif (data.buttons[4] == 1):
				joints.append("arm_base")
				positions.append(self.joint_states["arm_base"] + angle_change)
			elif (data.buttons[5] == 1):
				joints.append("arm_base")
				positions.append(self.joint_states["arm_base"] - angle_change)
			# arm_shoulder
			if (data.buttons[1] == 1 and data.buttons[2] == 1):
				# Do nothing they cancel out
				pass
			elif (data.buttons[1] == 1):
				joints.append("arm_shoulder")
				positions.append(self.joint_states["arm_shoulder"] + angle_change)
			elif (data.buttons[2] == 1):
				joints.append("arm_shoulder")
				positions.append(self.joint_states["arm_shoulder"] - angle_change)
			# arm_elbow
			if (data.buttons[0] == 1 and data.buttons[3] == 1):
				# Do nothing they cancel out
				pass
			elif (data.buttons[0] == 1):
				joints.append("arm_elbow")
				positions.append(self.joint_states["arm_elbow"] + angle_change)
			elif (data.buttons[3] == 1):
				joints.append("arm_elbow")
				positions.append(self.joint_states["arm_elbow"] - angle_change)
			# neck_tilt
			if (data.buttons[8] == 1 and data.buttons[9] == 1):
				# Do nothing they cancel out
				pass
			elif (data.buttons[8] == 1):
				joints.append("neck_tilt")
				positions.append(self.joint_states["neck_tilt"] + angle_change)
			elif (data.buttons[9] == 1):
				joints.append("neck_tilt")
				positions.append(self.joint_states["neck_tilt"] - angle_change)
			# neck_pan
			if (data.buttons[10] == 1 and data.buttons[11] == 1):
				# Do nothing they cancel out
				pass
			elif (data.buttons[10] == 1):
				joints.append("neck_pan")
				positions.append(self.joint_states["neck_pan"] + angle_change)
			elif (data.buttons[11] == 1):
				joints.append("neck_pan")
				positions.append(self.joint_states["neck_pan"] - angle_change)

		if (len(joints) > 0):
			cmd = JointState()
			cmd.header.stamp = rospy.get_rostime()
			cmd.name = joints
			cmd.position = positions
			self.armPub.publish(cmd)
					

if __name__ == '__main__':

	# In ROS, nodes are uniquely named. If two nodes with the same
	# node are launched, the previous one is kicked off. The
	# anonymous=True flag means that rospy will choose a unique
	# name for our 'listener' node so that multiple listeners can
	# run simultaneously.
	rospy.init_node('emu_joystick', anonymous=False)

	node = JoystickNode()

	# spin() simply keeps python from exiting until this node is stopped
	rospy.spin()
