import plotutils
import math


#STRATEGY = "Normal" 
#STRATEGY = "Boundary" 
#STRATEGY = "FullNeighborhood" 
RANGE = [-5.0, 5.0]
STEP_SIZE = 0.1
LATENCY = 0.05


robot.enable()
move_to_laser()


robot.set_motors_enabled(True)
current_y = robot_y.getPosition()
current_z = robot_z.getPosition()


r = bsearch([robot_y, robot_z], laser_distance,[RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [STEP_SIZE,STEP_SIZE], relative = True, maximum=True, strategy = STRATEGY, latency = LATENCY, title = "Binary Search YZ")


print r.print()
opt_y, opt_z= r.getOptimalPosition()
offset_y, offset_z = opt_y - current_y, opt_z - current_z

print "offset_y: ", offset_y, " offset_z: ", offset_z