#! /usr/bin/env mpscript
#

import os
import time
import math

# Suppress Pygame's built-in display window by using the SDL dummy display.

os.environ['SDL_VIDEODRIVER'] = 'dummy'

# Load and start pygame

import pygame

pygame.init()

# You have to give the dummy screen a size. 

screen = pygame.display.set_mode([1,1])

#==========================================================================

def print_usage():
	print \
"Usage: mp_joystick 'joystick_num' 'mtr1' 'mtr1_db' ['mtr2' 'mtr2_db' [...]]"

#==========================================================================

def main( mxdb, args ):

	num_joysticks = pygame.joystick.get_count()

	try:
		joystick_number = int( args[0] )
	except:
		print_usage();
		sys.exit(1);

	if ( joystick_number >= num_joysticks ):
		print \
	"Joystick number %d is outside the allowed range from 0 to %d" \
			% ( joystick_number, num_joysticks-1 )
		sys.exit(1)

	# Initialize the requested joystick.

	js = pygame.joystick.Joystick( joystick_number )

	js.init()

	# Get the requested axes from the remaining command line arguments.

	i = 1
	axis = 0
	js_motor_list = []

	while True:
		try:
			motor_name = args[i]
		except:
			# We have found all of the motors in the command line
			# arguments, so break out of the while loop.

			break

		try:
			js_deadband_text = args[i+1]
		except:
			print \
	"There should be an even number of arguments after the joystick number."
			sys.exit(1)

		js_deadband = float( js_deadband_text )

		# Does the specified motor exist in the MX database?

		motor_record = mxdb.get_record( motor_name )

		# Is this actually a motor record?

		record_class = motor_record.get_field('mx_class')

		if ( record_class != 'motor' ):
			print "MX record '%s' is not a motor record." \
				% ( motor_name )
			sys.exit(1)

		# Append this motor to the list.

		js_motor = JoystickMotor( motor_record, js_deadband )

		js_motor_list.append( js_motor )

		#----

		i += 2
		axis += 1

	num_axes = axis

	name_list = []

	for jsm in js_motor_list:
		name_list.append( jsm.motor.get_field('name') )

	print "MX joystick handler ready for motors %s" % ( name_list )

	#------------------------------------------------------------------
	# Loop forever waiting for events.

	debug = False
	report_move = True

	while True:

		# Parse and handle the events.

		for event in pygame.event.get():

			if ( event.type == pygame.JOYAXISMOTION ):
				js_axis = event.dict['axis']
				js_axis_value = event.dict['value']

				if debug:
					print "axis %d, position = %g" \
						% ( js_axis, js_axis_value )

				if ( js_axis < num_axes ):
				    if report_move:
					print \
					"Moving '%s', js_axis_value = %f" \
					% ( name_list[js_axis], js_axis_value )

				    js_motor_list[js_axis].move( js_axis_value )

			elif ( event.type == pygame.JOYBALLMOTION ):
				if debug:
					print "Trackball motion"
					print "event.dict =", event.dict

			elif ( event.type == pygame.JOYBUTTONDOWN ):
				if debug:
					js_button = event.dict['button']

					print "Button %d down" % js_button

			elif ( event.type == pygame.JOYBUTTONUP ):
				if debug:
					js_button = event.dict['button']

					print "Button %d up" % js_button

			elif ( event.type == pygame.JOYHATMOTION ):
				if debug:
					js_hat = event.dict['hat']
					js_hat_value = event.dict['value']

					print "Hat %d value = %s" \
						% ( js_hat, js_hat_value )
			else:
				if debug:
					print "Non-joystick Pygame event"
					print "event =", event

		time.sleep(0.1)

#==========================================================================

class JoystickMotor:
	def __init__( self, motor, js_deadband ):
		self.motor = motor
		self.js_deadband = js_deadband

		self.motor.soft_abort()

	def move( self, move_value ):
		moving = self.motor.get_status() & 0x1

		# print "moving =", moving

		if ( math.fabs(move_value) < self.js_deadband ):
			if ( moving ):
				# print "*** Soft abort ***"
				self.motor.soft_abort()
		else:
			if ( moving == 0 ):
				if ( move_value >= 0.0 ):
					# print "*** Move positive ***"
					self.motor.constant_velocity_move( 1 )
				else:
					# print "*** Move negative ***"
					self.motor.constant_velocity_move( -1 )