
# Joystick Controller for Invivo AI challenge simulation environment
# Copyright 2009,2010 Gerard Toonstra, Simon Funk
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program.  If not, see <http://www.gnu.org/licenses/>.

import math
from pygame import joystick

class Controller(object):

	"""Joystick (interactive) controller for Interface01.
	"""
	def __init__(self, interfaces):
		"""Interfaces is a pair of Interface01 objects."""
		self.bots = interfaces
		joystick.init()
		if ( joystick.get_count() > 0 ):
			self.joystick = joystick.Joystick( 0 )
			self.joystick.init()
		else:
			self.joystick = None

	def update(self, time):
		if ( self.joystick == None ):
			return

		if ( self.bots[ 0 ].ballpos[ 0 ] > 0.0 ):
			self.controlBot( self.bots[ 1 ] )
		else:
			self.controlBot( self.bots[ 0 ] )

	def controlBot( self, bot ):
		bot.torques[ 0 ] = -4.0 * self.joystick.get_axis( 0 )
		bot.torques[ 1 ] = -2.0 * self.joystick.get_axis( 1 )
		bot.torques[ 2 ] = -1.0 * self.joystick.get_axis( 2 )

