################################################################################################### 
# Using bsearch(Binary Search) and hsearch(Hill Climbing Search) to find optimum
################################################################################################### 
if get_device("image_stats") is None:    
    run("camtool")
    image_stats.start()
    image_stats.setNumberOfImages(5)
    laser_off()   
    image_stats.captureBackground(10)
    laser_on()   
    image_stats.setNumberOfImages(5)
    image_stats.enableBackground
(True)

def before_sample():    
    image_stats.update()

class FitnessFunction(ReadonlyRegisterBase):
    def doRead(self):
        r = lscan(gun_solenoid, [image_stats.com_x_mean, image_stats.com_y_mean] , 110.0, 120.0, 1.0, 0.1, before_read = before_sample)
        bounding_area = abs(max(r.getReadable(0)) - min(r.getReadable(0))) * abs(max(r.getReadable(1)) - min(r.getReadable(1)))
        return bounding_area

add_device(FitnessFunction("fitness"), True)            



"""
#Binary Search
strategy = "Normal" # or "Boundary" or "FullNeighborhood"
r = bsearch([ao1, ao2], fitness, [0.0,0.0], [21.0,26.0], [0.1, 0.1], maximum=True, strategy = strategy, latency = 0.01, title = "Binary Search")

print "---------------  Binary Search  -----------------"
print r
print r.print()
print len(r.getRecords())

"""

#Hill Climbing Search
#r = hsearch([gsx, gsy, gsrx, gsry], fitness,[-1.0,-1.0,-1.0,-1.0], [1.0,1.0,1.0,1.0], [0.1, 0.1, 0.1, 0.1], [0.01, 0.01, 0.01, 0.01], 3, relative = False, maximum=False, latency = 0.01, title = "Hill Climbing")
r = hsearch([gsx,gsy,gsrx, gsry], fitness,[-1.0,-1.0,-1.0,-1.0], [1.0,1.0,1.0,1.0], [0.1,0.1, 0.1, 0.1], [0.01,0.01,0.01, 0.01], 2, relative = False, maximum=False, latency = 0.01, title = "Hill Climbing")

print "---------------  Hill Climbing Search  -----------------"
print r
print r.print()
print len(r.getRecords())