from os import sys
from inputs import get_gamepad
from drive import RosAriaDriver
from queue import Queue
from threading import Thread

class PioneerX360():

	def __init__(self, n):
		self.pad = get_gamepad()
		self.pioneer = RosAriaDriver('PIONIER' + n + '/')
		self.axes = {
			"Y": 0,
			"X": 0
		}
		self.mapper = {
			# "BTN_TL": if_button_pressed(pioneer.GripperUp),
			# "BTN_TR": if_button_pressed(pioneer.GripperDown),
			# "BTN_SOUTH": if_button_pressed(pioneer.GripperOpen),
			# "BTN_WEST": if_button_pressed(pioneer.GripperClose),
			# "ABS_Y": go_T,
			"BTN_MODE": exit,
		}


	def if_button_pressed(fun):
		def foo(state):
			if state == 1:
				fun()
		return foo

	def update_axes(self, event):
		axis = event.code[4:]
		deviation = event.state
		self.axes[axis] = deviation


	def publish_cmd(self):
		print(self.axes)
		if self.axes["Y"] < -10000:
			self.pioneer.SetSpeed(0.1, 0, 0)
		elif self.axes["Y"] > 10000:
			self.pioneer.SetSpeed(-0.1, 0, 0)
		else:
			self.pioneer.SetSpeed(0, 0, 0)

	def run(self):
		while 1:
			events = get_gamepad()
			self.publish_cmd()
			for event in events:
				self.publish_cmd()
				if event.code[0:3] == "ABS":
					self.update_axes(event)
				else:
					try:
						self.mapper[event.code](event.state)
					except KeyError:
						pass
						# print(event.code, event.state)

p = PioneerX360(sys.argv[1])
p.run()