import mathutils
import plotutils

SIMULATION =  True

X_MIN = scx.minValue / 2
X_MAX = scx.maxValue / 2
Y_MIN = scy.minValue / 2
Y_MAX = scy.maxValue / 2

MAX_SAMPLE_SIZE = 0.4
MIN_SAMPLE_SIZE = 0.25
SCAN_RESOLUTION = 0.01
OMEGA_START = 57.0

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)

step=0
for y in frange(Y_MIN, Y_MAX, y_step):   
    scy.move(y)
    scan_range = [X_MIN, X_MAX] if (step % 2==0) else [X_MAX, X_MIN]
    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, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
        if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
            raise Exception("Invalid x fit")
        plot_fit(normalization, mean_val, sigma, xdata)        
        print "Found x center at ", mean_val
        scx.move(mean_val)
        break        
    except:        
        print "Invalid x fit for y=", y, ": ", sys.exc_info()[0]
        
    step += 1

r = cscan(scy, sensor, y-MAX_SAMPLE_SIZE/2, Y_MAX+MAX_SAMPLE_SIZE/2, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
(offset, normalization, mean_val, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
    raise Exception("Invalid x fit")
plot_fit(normalization, mean_val, sigma, xdata)    
print "Found y center at ", mean_val   
scy.move(mean_val)

print "Success finding center at x=", scx.read(), " y=", scy.read()


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)        
(offset, normalization, mean_minus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
    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_MIN, X_MAX, SCAN_RESOLUTION, latency = DIODE_SETTLING_TIME)
xdata,ydata = enforce_monotonic(r.getPositions(0)), r.getReadable(0)        
(offset, normalization, mean_plus_15, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
if ((normalization-offset) < offset * 1.20) or (sigma > 0.5):
    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)

print "Centering successfull"