import plotutils
import math


#STRATEGY = "Normal" 
STRATEGY = "Boundary" 
#STRATEGY = "FullNeighborhood" 
RANGE = [-5.0, 5.0]
INITIAL_STEP = 1.0
STEP_SIZE = 0.05
LATENCY = 0.05
NOISE_FILTER = 1


robot.enable()
move_to_laser()


robot.set_motors_enabled(True)

current_x = robot_x.getPosition()
current_z = robot_z.getPosition()




class Distance(Readable):
    def read(self):
        ret = ue.readable.read()
        ret = 0.0 if math.isnan(ret) else ret
        return ret
        
laser_distance=Distance()   
start = time.time()        
r = hsearch([robot_x, robot_z],laser_distance, [RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [INITIAL_STEP,INITIAL_STEP], [STEP_SIZE,STEP_SIZE], NOISE_FILTER, relative = True, maximum=True, latency = LATENCY, title = "Hill Climbing 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