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_x = robot_x.getPosition()
current_z = robot_z.getPosition()


r = bsearch([robot_x, 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 XZ")


print r.print()
opt_x, opt_z= r.getOptimalPosition()
offset_x, offset_z = opt_x - current_x, opt_z - current_z

print "offset_x: ", offset_x, " offset_z: ", offset_z