import mathutils
import plotutils


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

SIMULATION =  True
Y_STEP = 0.125


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)

for y in frange(Y_MIN, Y_MAX, Y_STEP):   
    scy.move(y)
    r = cscan(scx, sensor, scx.minValue, scx.maxValue, 0.01, latency = DIODE_SETTLING_TIME)

    try:
        xdata = enforce_monotonic(r.getPositions(0))
        ydata = r.getReadable(0)
        (offset, normalization, mean_val, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        

        if (normalization-offset) < offset * 1.20:
            raise Exception("Invalid x fit")
        if sigma > 0.5:
            raise Exception("Invalid x fit")
        #Plotting the gaussian
        gaussian = mathutils.Gaussian(normalization, mean_val, sigma)
        plotutils.plot_function(get_plots(None)[0], gaussian, "Fit", xdata)

        print "Found x center at ", mean_val
        scx.move(mean_val)
        break        
    except:
        print "Invalid x fit for y=", y


r = cscan(scy, sensor, scy.minValue, scy.maxValue, 0.01, latency = DIODE_SETTLING_TIME)
xdata = enforce_monotonic(r.getPositions(0))
ydata = r.getReadable(0)
(offset, normalization, mean_val, sigma) = mathutils.fit_gaussian_offset(ydata, xdata)        
if (normalization-offset) < offset * 1.20:
    raise Exception("Invalid y fit")
if sigma > 0.5:
    raise Exception("Invalid y fit")
print "Found y center at ", mean_val   
scy.move(mean_val)

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