MAX_SHIFT = 300
SLEEP_MOVE = 3.0
CALIBRATE_RANGE = 20.0

shift_ref = shift_ref_x = shift_ref_y = None

def get_image_shift(cur_data=None, former_data=None, roi=None):
    global shift_ref
    if roi is None:
        roi=get_focus_scan_roi()
    if roi is None:
        roi = Rectangle(128,128,256,256)
    if former_data is None:
        former_data = shift_ref
    if former_data is None:
         raise Exception("Image reference not available")  
    if cur_data is None:
        cur_data = get_cur_data()
    print roi
    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):
            return  -xoff, -yoff  
        else:
            raise Exception("Error - shift too big: " + str(xoff) + "," + str(yoff))
    except:
        raise Exception("Error calculating shift: " + str(sys.exc_info()[1]))

def get_cur_data():
    return image.data
    

def grab_shift_ref():
    global shift_ref, shift_ref_x, shift_ref_y
    #TODO: read from device
    obj_align_x.write(0.0);obj_align_y.write(0.0)
    time.sleep(SLEEP_MOVE)
    shift_ref_x = shift_ref_y = 0.0
    
    shift_ref = get_cur_data()

def calibrate_image_shift():


    #TODO: Must find way to read current OBJDX and OBJDY values      
    obj_align_y.write(0.0) 
    obj_align_x.write(-CALIBRATE_RANGE) 
    time.sleep(SLEEP_MOVE)
    grab_shift_ref()
    obj_align_x.write(CALIBRATE_RANGE) 
    time.sleep(SLEEP_MOVE)
    xdx, xdy = get_image_shift()

    obj_align_x.write(0.0) 
    obj_align_y.write(-CALIBRATE_RANGE) 
    time.sleep(SLEEP_MOVE)
    grab_shift_ref()
    obj_align_y.write(CALIBRATE_RANGE) 
    time.sleep(SLEEP_MOVE)
    ydx, ydy = get_image_shift()
    obj_align_x.write(0.0) 
    obj_align_y.write(0.0) 
    time.sleep(SLEEP_MOVE)

    #TODO calcs are wrong
    #s = hipot(mdy,mdx) / hipot(idt,idx)
    # cos beta = mdx/mdy
    # th alpha = idy/idx
    #teta = alpha - beta

    
    ixt, iyt = math.hypot(xdx, xdy), math.hypot(ydx, ydy) #image move
    mdx= mdy = 2 * 10.0 #Total motor move
    sx,sy = ixt/mdx,  iyt/mdy  #Scale
    s = mean ((sx,sy)) #supposing x and y scale equal
    #angle
    t1 = math.acos((-xdx/sx)/mdx)
    t2 = math.asin((xdy/sx)/mdx)
    t3 = math.acos((ydy/sy)/mdy)
    t4 = math.asin((ydx/sy)/mdy)
    theta = mean((t1,t2,t3,t4))

    set_setting("image_shift_angle", theta)
    set_setting("image_shift_scale", s)
    print s, math.degrees(theta)    
    
    
def correct_image_shift():
    angle = get_setting("image_shift_angle")        
    scale = get_setting("image_shift_scale")
    if (angle is None) or (scale is None) :
        raise Exception("Image shift not calibrated")
  
    angle, scale = float(angle), float(scale)
    idx, idy = get_image_shift()
    #idx, idy  = idx/scale, idy/scale
     
    
    it = math.hypot(idx, idy)
    #mdx = idx / math.cos(angle) 
    #mdy = idy / math.sin(angle) 
    mt = it/scale
    #mdx = mt * math.sin(angle)
    #mdy = mt * math.cos(angle)

    try:
        t=math.atan(idy/idx)
        t2 = t+angle
        #mdx = it * math.cos(-t2)
        #mdy = it * math.sin(-t2)  
        
        mdx = idx * math.cos(t2) 
        mdy = idy * math.sin(t2)      
        #mdx = mt * math.cos(t2) 
        #mdy = mt * math.sin(t2)                 
    except: 
        #ZeroDivisionError
        mdx = mdy = 0.0
        

    mdx=mdx+shift_ref_x
    mdy=mdy+shift_ref_y
    print "Moving " , mdx, ", ",  mdy

def translate(shift, image):
    #shifts= load_shifts("{images}/TestObjAligner/shifts.mat")
    stack = load_test_stack(show=True)
    r=translate(stack, shifts, show=True)    
    
#
#grab_shift_ref()
#print get_image_shift()
#calibrate_image_shift()