# A bare brain

from pyro.brain import Brain

class BrainA(Brain):
	# Only method you have to define is the step method:

	def setup(self):  # constructor
		# create any vars you need here
		self.robot.set('/robot/range/units', 'METERS')

	def destroy(self): # destructor
		pass
		

	def step(self):  # called for each simulation step
		angle = 0
		speed = 0.5
		
		if (self.robot.get('/robot/range/0/value') > self.robot.get('/robot/range/2/value')): # if closer obstacle is on the left
			angle = -(1-(self.robot.get('/robot/range/2/value') / 1.6))
		elif (self.robot.get('/robot/range/2/value') > self.robot.get('/robot/range/0/value')): # if closer obstacle is on the right
			angle = (1-(self.robot.get('/robot/range/0/value') / 1.6))

		if (self.robot.get('/robot/range/1/value') < 1.2):
			speed = -1
			angle = 3*angle
			if (angle > -0.5 and angle < 0.5):
				if (angle < 0):
					angle = 0.5
				else:
					angle = -0.5
			if angle > 1:
				angle = 1
			elif angle < -1:
				angle = -1

		print "move("+str(speed)+', '+str(angle)+')'
		self.robot.move(speed, angle) # negative is to the right!

#		print str(self.robot.get('/robot/range/0/value'))+"; "+str(self.robot.get('/robot/range/1/value'))+"; "+str(self.robot.get('/robot/range/2/value'))+"; angle="+str(angle)


		# position data - dead reconing
#		print str(self.robot.get('robot/x'))+','+str(self.robot.get('robot/y'))+','+str(self.robot.get('robot/th'))

# -------------------------------------------------------
# This is the interface for calling from the gui engine.
# Takes one param (the robot), and returns a Brain object:
# -------------------------------------------------------
def INIT(engine):
	return BrainA("BrainA", engine)
	print engine.robot.name + " robot now has " + brain.name + " brain."      


# NOTES ON FUNCTION USE
# self.robot.move(speed(+=forward), direction(-=left))