import mathutils
import plotutils

if  get_exec_pars().source == CommandSource.ui:
    SIMULATION =  True    
    X_MIN = scx.minValue / 2
    X_MAX = scx.maxValue / 2
    Y_MIN = scy.minValue / 2
    Y_MAX = scy.maxValue / 2    
    MIN_SAMPLE_SIZE = 0.15
    SCAN_RESOLUTION = 0.01
    OMEGA_START = 57.0
    Z_START = 0.0
    
MAX_SAMPLE_SIZE = 0.5

    
y_step = MIN_SAMPLE_SIZE * 0.5
sensor = scd_sim if SIMULATION else scd


X_MIN, X_MAX, Y_MIN, Y_MAX = float(X_MIN), float(X_MAX), float(Y_MIN), float(Y_MAX)

def plot_fit(normalization, mean_val, sigma, xdata):
    gaussian = mathutils.Gaussian(normalization, mean_val, sigma)
    plotutils.plot_function(get_plots(None)[0], gaussian, "Fit", xdata)

scr.move(OMEGA_START)
scz.move(Z_START)

step=0
for y in frange(Y_MIN, Y_MAX, y_step):   
    scan_range = [X_MIN, X_MAX] if (step % 2==0) else [X_MAX, X_MIN]
    scx.moveAsync(scan_range[0])
    scy.move(y)    
    r = cscan(scx, sensor, scan_range[0], scan_range[1], SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)

    try:
        xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
        (offset, normalization, mean_val_x, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
        print "Fit: " + str ((offset, normalization, mean_val_x, sigma))
        peak_height = normalization+offset 
        print "peak_height=" + str(peak_height) 
        if (peak_height < (offset * 1.20)) or (sigma < y_step ):
            raise Exception("Invalid x fit ")
        plot_fit(normalization, mean_val_x, sigma, xdata)        
        print "Found x center at ", mean_val_x
        scx.move(mean_val_x)
        break        
    except:        
        print "Invalid x fit for y=", y, ": ", sys.exc_info()[1]
        
    step += 1

#r = cscan(scy, sensor, max(y-MAX_SAMPLE_SIZE/2, scy.minValue), min(Y_MAX+MAX_SAMPLE_SIZE/2, scy.maxValue), SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
r = cscan(scy, sensor, Y_MIN, Y_MAX, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
try:
    (offset, normalization, mean_val_y, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
except:    
    raise Exception ("Invalid y fit: ", sys.exc_info()[0])    
peak_height = normalization+offset 
print "peak_height=" + str(peak_height) 
if (peak_height < (offset * 1.20)) or (sigma < y_step ):  
    raise Exception("Invalid y fit")
plot_fit(normalization, mean_val_y, sigma, xdata)    
print "Found y center at ", mean_val_y   
scy.move(mean_val_y)

print "Success finding center at x=", mean_val_x, " y=", mean_val_y


try:
    scr.move(OMEGA_START-15.0)    
    r = cscan(scx, sensor, X_MIN, X_MAX, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
    xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
    try:
        (offset, normalization, mean_minus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
    except:    
        raise Exception ("Invalid x fit on z search -15deg: ", sys.exc_info()[0])            
    peak_height = normalization+offset 
    print "peak_height=" + str(peak_height) 
    if (peak_height < (offset * 1.20)) or (sigma < y_step ):  
        raise Exception("Invalid x fit on z search -15deg")
    plot_fit(normalization, mean_minus_15, sigma, xdata)    
    print "Found x center at -15deg at ", mean_minus_15   
    
    
    scr.move(OMEGA_START+15.0)    
    r = cscan(scx, sensor, X_MAX, X_MIN, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
    xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
    try:
        (offset, normalization, mean_plus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
    except:    
        raise Exception ("Invalid x fit on z search +15deg: ", sys.exc_info()[0])        
    peak_height = normalization+offset 
    print "peak_height=" + str(peak_height) 
    if (peak_height < (offset * 1.20)) or (sigma < y_step ):  
        raise Exception("Invalid x fit on z search +15deg")
    plot_fit(normalization, mean_minus_15, sigma, xdata)    
    print "Found x center at +15deg at ", mean_plus_15
    
    scr.move(OMEGA_START)    
    
    z_offset = 0.5 * (mean_minus_15 - mean_plus_15) / math.tan(math.radians(15.0))
    print "Calculated z offset = ", z_offset
    scz.moveRel(z_offset)
finally:
    scx.move(mean_val_x)
    scy.move(mean_val_y)
    
print "Centering successfull"