#!/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

BUTTON_A = 1
BUTTON_B = 2
BUTTON_Y = 3
BUTTON_X = 0
BUTTON_LT = 6
BUTTON_LB = 4
BUTTON_RT = 7
BUTTON_RB = 5
BUTTON_START = 9
BUTTON_BACK = 8
BUTTON_LEFT_STICK = 10
BUTTON_RIGHT_STICK = 11

class JoystickNode:


	def __init__(self):
		self.motors_stopped = False
		self.flipper_stopped = False
		self.dying = False
		rospy.Subscriber("joy", Joy, self.callback)
		self.twistPub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
		self.flipperPub = rospy.Publisher("/joint_control", JointState, queue_size=1)
		self.max_speed = rospy.get_param('~speed', 0.5)
		self.max_turn = rospy.get_param('~turn', 0.5)


	def setVelocities(self, forward, turn):
#		rospy.loginfo(rospy.get_caller_id() + " Setting velocities %f,%f", 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 driveFlipper(self, speed):
#		rospy.loginfo(rospy.get_caller_id() + " Driving flipper %f", speed)
		cmd = JointState()
		cmd.header.stamp = rospy.get_rostime()
		cmd.name.append("flipper")
		cmd.position.append(float('inf'))
		cmd.velocity.append(speed)
		self.flipperPub.publish(cmd)

	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)

		if data.buttons[BUTTON_LT] == 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
		
		if data.buttons[BUTTON_Y] == 1:
			self.flipper_stopped = False
			self.driveFlipper(1);
		elif data.buttons[BUTTON_X] == 1:			
			self.flipper_stopped = False
			self.driveFlipper(-1)
		elif not self.flipper_stopped:
			self.driveFlipper(0)
			self.flipper_stopped = True
		
		if (data.buttons[BUTTON_B] == 1 and data.buttons[BUTTON_LB] == 1 and data.buttons[BUTTON_RB] == 1 and data.buttons[BUTTON_RT] == 1):
			# wireless D/off -> A, RT, back, start
			self.dying = True
			self.setVelocities(0,0)
			self.driveFlipper(0)
			rospy.logwarn(rospy.get_caller_id() + " Killing roslaunches.")
			os.system("killall -s SIGINT roslaunch")

		
		
		if (data.buttons[BUTTON_LT] == 0 and data.buttons[BUTTON_START] == 1 and data.buttons[BUTTON_RT] == 1 and data.buttons[BUTTON_LB] == 1 and data.buttons[BUTTON_RB] == 1):
			# wireless D/off -> B, LB, RT, start, left stick
			self.dying = True
			self.setVelocities(0,0)
			self.driveFlipper(0)
			rospy.logwarn(rospy.get_caller_id() + " Shutting down the machine.")
			os.system("sudo shutdown -P now")
#TODO			os.system("killall -s SIGINT openday.sh")
			os.system("killall -s SIGINT roslaunch")


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('negotiator_joystick', anonymous=False)

	node = JoystickNode()

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