from mathutils import fit_polynomial
from mathutils import PolynomialFunction
import math
from plotutils import plot_function
print "Starting"


#Creating averaging devices
av_hamp_x = create_averager(hamp_x, count = 50, interval = -1, name = "av_hamp_x")
av_xbpm_x = create_averager(xbpm_x, count = 50, interval = -1, name = "av_xbpm_x")
#av_hamp_x.monitored = True
av_xbpm_x.monitored = True

#The actuals scan
r=lscan(pbpg_mx, [av_hamp_x, av_xbpm_x], -1.5, 1.5, 100, latency = 0.0)

#Fitting 
values = to_array(r.getReadable(0), 'd')
positions = r.getPositions(0)
try:
    pars_polynomial  = fit_polynomial(values, positions, 1)
except:
    print "Fit error"
    raise

print "Plolynomial pars: ", pars_polynomial
(a0, a1)  = pars_polynomial 
#Writing metadata to data file
path = get_exec_pars().scanPath
print "Scn path: ", path
set_attribute(path, "a0", a0)      
set_attribute(path, "a1", a1)      
#set_attribute(path, "a2", a2)      

#Plotting fit and writing fitting parameters
outp = "a0="+ ("%0.4f" % a0) + "a1="+  ("%0.4f" % a1)
print outp
p = get_plots()[0]
p.addText((min(positions) + max(positions))/2, max(values), outp, Color.BLACK)
plot_function(p, PolynomialFunction(pars_polynomial), "Fit",positions, show_points = False, show_lines = True, color = Color.BLUE)