def __init__(self):

		# set working directory
		abspath = os.path.abspath(__file__)
		dname = os.path.dirname(abspath)
		os.chdir(dname)
		print('Current directory: ' + subprocess.check_output('pwd').decode("utf-8"))

		# setting up motors
		print('Initializing motors')
		ports = faulhaber_port_numbers()
		self.motors = Motors(ports)

		# seting up camera
		print('Initializing camera')
		self.camera =  Camera(
			starting_exposure = 2745,
			color_mode = ids.ids_core.COLOR_BAYER_16
			)

		# setting up arduino communication
		print('Initializing arduino connection')
		#self.arduino_conn = arduino_connection()

		self.cluster_size_threshold = 1000
		self.by_size = False
		self.nominal_laser_intensity = 41
		self.correct_for_laser = True

		print('Initializing mologram position control')
		self.chip = Chip(self.motors)

		print('Done')

		print('System booted normally')		
class Setup:

	def __init__(self):

		# set working directory
		abspath = os.path.abspath(__file__)
		dname = os.path.dirname(abspath)
		os.chdir(dname)
		print('Current directory: ' + subprocess.check_output('pwd').decode("utf-8"))

		# setting up motors
		print('Initializing motors')
		ports = faulhaber_port_numbers()
		self.motors = Motors(ports)

		# seting up camera
		print('Initializing camera')
		self.camera =  Camera(
			starting_exposure = 2745,
			color_mode = ids.ids_core.COLOR_BAYER_16
			)

		# setting up arduino communication
		print('Initializing arduino connection')
		#self.arduino_conn = arduino_connection()

		self.cluster_size_threshold = 1000
		self.by_size = False
		self.nominal_laser_intensity = 41
		self.correct_for_laser = True

		print('Initializing mologram position control')
		self.chip = Chip(self.motors)

		print('Done')

		print('System booted normally')		

	def photodiode_intensity(self):
		val = self.arduino_conn.read()
		val = ord(val)

		return val

	def calib(self):
		self.motors.calib()
		self.chip.move_to('B,1,1')

	def binary_search(
		self,
		increment,
		evaluate,
		starting_step,
		final_step,
		starting_direction,
		condition,
		max_difference
		):
		
		# initialize search parameters
		step = starting_step
		direction = starting_direction
		
		new_score, old_score, i = 0,0,0

		# divide
		while (abs(step) > final_step):
			
			# initialize score
			old_score = evaluate()
			increment(direction*step)
			new_score = evaluate()
				
			print('Exposure: ' + str(self.camera.exposure) + ', step: ' + str(step))
			print('New score: ' + str(new_score) + ', old score: ' + str(old_score))
			
			# keep incrementing until reach a local extrema
			while condition(old_score,new_score,direction,step):
				
				increment(direction*step)

				old_score = new_score
				new_score = evaluate()
				
				buffer_length = 3
				for i in range(buffer_length-1):
					new_scores[i] = old_scores[i+1]

				new_scores[buffer_length-1] = new_score				
				
				print('Exposure: ' + str(self.camera.exposure) + ', step: ' + str(step))
				print('New score: ' + str(new_score) + ', old score: ' + str(old_score))
			
			# update search parameters
			step *= 0.5
			direction *= -1
			
			i += 1
			if i > 100:
				return

	def max_pixel(self):
		num_pixels = 50
		buffer_length = 5
		values = np.empty(buffer_length)
		
		for i in range(buffer_length):  
			img,retval = self.camera.get_raw_img()
			values[i] = np.mean(heapq.nlargest(num_pixels, img.flatten()))
			#values[i] = np.max(img.flatten())
			
		value = np.mean(values)
		
		return value

	def adjust_exposure(self):
		saturation_value = 255
		margin = 1

		final_step = 0.5
		max_difference = 5
		
		# condition = lambda old_score,new_score,direction: \
		#     (new_score >= old_score - margin) and not(new_score == 255 and direction == 1)
		
		condition = lambda old_score,new_score,direction,step: \
			(((new_score < saturation_value/2) and (direction ==  1))  or \
			((new_score > saturation_value/2) and (direction == -1)))  and \
			(abs(step) > final_step)

		
		self.binary_search(
			increment = lambda x: self.camera.set_exposure(self.camera.exposure + x),
			evaluate  = lambda: self.mologram_intensity(avg = True)[0],
			starting_step = 64,
			final_step = final_step,
			starting_direction = -1,
			condition = condition,
			max_difference = 5
		)
		
	def adjust_focus(self):
		saturation_value = 255
		margin           = 20000
		max_difference   = 5000
		final_step       = 1
		starting_step    = 40

		evaluate  = lambda: processing.image_score(self.camera.get_raw_img()[0])
		increment = lambda x: self.motors.z.move_um(x)

		init_eval = evaluate()
		
		condition = lambda old_score,new_score,direction,step: \
			(new_score <= old_score + margin) #and \
			#((direction == 1) and (new_score >= saturation_value))
		
		self.binary_search(
			increment = increment,
			evaluate  = evaluate,
			starting_step = starting_step,
			final_step = final_step,
			starting_direction = 1,
			condition = condition,
			max_difference = max_difference
		)

		if evaluate() < init_eval:
			self.binary_search(
				increment = increment,
				evaluate  = evaluate,
				starting_step = starting_step,
				final_step = final_step,
				starting_direction = -1,
				condition = condition,
				max_difference = max_difference
			)
		
	def adjust_coupling(self):
		saturation_value = 255
		margin = 5
		
		condition = lambda old_score,new_score,direction,step: \
			(new_score >= old_score - margin) and \
			((direction == 1) and (new_score >= saturation_value))
		
		self.binary_search(
			increment = lambda x: self.motors.TE.move_degrees(x),
			evaluate  = lambda: self.mologram_intensity(avg = True)[0],
			starting_step = 0.1,
			final_step = 0.01,
			starting_direction = 1,
			condition = condition,
			max_difference = 1
		)