#wold be nice to print relative changes of the parameters
RANGE_OBJ = 1.0
STEP_OBJ = 0.1
RANGE_STIG = 10.0
STEP_STIG = 1.0
UPDATE_POSITION = True
AVERAGE = 1
width, height = eiger.getImageSize()
#ROI = Rectangle(width/3, height/3, width/3, height/3,)
#ROI = Rectangle(0, 0, width, height,)
ROI = Rectangle(200,100,256,256)#(x0,y0,delta_x(2**n), delta_y(2**n))

initial_state = objective.read(),obj_stig_a.read(),obj_stig_b.read()    
print "Initial state:" , initial_state

def apply_pos(pos):
    objective.write(pos[0])
    obj_stig_a.write(pos[1])
    obj_stig_b.write(pos[2])

def show_roi():
    global ROI
    pn=show_panel(image)
    ov=Overlays.Rect(Pen(Color.BLUE), Point(ROI.x,ROI.y),Dimension(ROI.width,ROI.height))
    pn.clearOverlays()
    pn.addOverlay(ov)
show_roi()
    
MAX_SHIFT = 20
former_data = image.data
def update_roi():
    global former_data, ROI
    cur_data = image.data
    calc_roi = Roi(ROI.x,ROI.y,ROI.width, ROI.height)
    try:
        xoff, yoff, error, diffphase, _ = calculate_shift(former_data,cur_data, calc_roi)
        print "Calculated shift: ", xoff, yoff, error, diffphase
        if (abs(xoff) <= MAX_SHIFT) and (abs(yoff) <= MAX_SHIFT):
            x,y=int(ROI.x - xoff), int(ROI.y - yoff)
            print "Updating ROI location to ", x, y
            ROI.setLocation(x,y)
            show_roi()
    except:
        print "Error calculating shift: " + str(sys.exc_info()[1])
    former_data = cur_data
    

r=scan_focus(objective, RANGE_OBJ, STEP_OBJ, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
update_roi()
# r=scan_focus(obj_stig_a, RANGE_STIG, STEP_STIG, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
# update_roi()
# r=scan_focus(obj_stig_b, RANGE_STIG, STEP_STIG, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
# update_roi()
# r=scan_focus(objective, RANGE_OBJ, STEP_OBJ, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
# update_roi()
# r=scan_focus(obj_stig_a, RANGE_STIG, STEP_STIG, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
# update_roi()
# r=scan_focus(obj_stig_b, RANGE_STIG, STEP_STIG, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)
# update_roi()
# r=scan_focus(objective, RANGE_OBJ, STEP_OBJ, roi = ROI, average = AVERAGE, update_position = UPDATE_POSITION)

final_state = objective.read(),obj_stig_a.read(),obj_stig_b.read()    
print "Final state:" , final_state