################################################################################################### 
# Using bsearch(Binary Search) and hsearch(Hill Climbing Search) to find optimum
################################################################################################### 

class Sensor(ReadonlyRegisterBase):
    def doRead(self):
        return 1000.0 - (math.pow(motor.take()-0.5, 2) + math.pow(pe.take()+2, 2))
add_device(Sensor("sensor"), True)    

range_x, step_x=1.0, 0.1
range_y, step_y=10.0, 0.1

motor.speed=5.0
motor.move(0.0)
pe.move(0.0) 

print "---------------  Full 2D Scan  -----------------"
r = ascan([motor, pe], sensor, [-range_x,-range_y], [range_x,range_y], [step_x, step_x], relative=True, zigzag=True)
data = Convert.reshape(r[sensor], r.scan.numberOfSteps[0]+1, r.scan.numberOfSteps[1]+1)
data = [(data[i][::-1] if ((i%2)==1) else data[i]) for i in range(len(data))]
d=Data(to_array(data,'d'))
v,h=d.integrateVertically(), d.integrateHorizontally()
mv=fit(v)[1]
mh=fit(h)[1]
index = int(round(mh))*(r.scan.numberOfSteps[1]+1) + int(round(mv))
max_pe, max_motor= r[pe][index], r[motor][index]
print "2s scan peak at motor=" , max_motor , " pe=", max_pe
motor.move(max_motor)
pe.move(max_pe)

print "---------------  Binary Search  -----------------"
motor.move(0.0);pe.move(0.0) 
r = bsearch([motor, pe], sensor, [-range_x,-range_y], [range_x,range_y], [step_x/4, step_y/4], \
    relative=True, maximum=True, strategy = "Normal", restore_position=False )
print "bsearch peak at position=",  r.optimalPosition , " in ", len(r), "evaluations"

motor.move(0.0);pe.move(0.0) 
print "---------------  Hill Climbing Search  -----------------"
r = hsearch([motor, pe], sensor, [-range_x,-range_y],[range_x,range_y], [step_x*2, step_y*2], \
    [step_x/4, step_y/4], relative=True, maximum=True, restore_position=False )
print "hsearch peak at position=",  r.optimalPosition, " in ", len(r), "evaluations"