################################################################################################### 
# Least square optimization problem to fit a gaussian with offset  
################################################################################################### 

from mathutils import *
from plotutils import * 

run("CPython/GaussFit_wrapper")
################################################################################################### 
#Fitting the gaussian function with offset f(x) = a + b * exp(-(pow((x - c), 2) / (2 * pow(d, 2))))
################################################################################################### 

camtool.start("Simulation")

time.sleep(2.0)
xdata =camtool.getValue("x_axis")
ydata = camtool.getValue("x_profile")


p = plot(ydata, "Data" , xdata)[0]
p.setLegendVisible(True)

def fit_gaussian_offset(y, x, start_point = None, weights = None):
    """Fits data into a gaussian with offset.
       f(x) = a + b * exp(-(pow((x - c), 2) / (2 * pow(d, 2))))

    Args:
        x(float array or list): observed points x 
        y(float array or list): observed points y 
        start_point(optional tuple of float): initial parameters (normalization, mean, sigma)
        weights(optional float array or list): weight for each observed point
    Returns:
        Tuples of gaussian parameters: (offset, normalization, mean, sigma)
    """     

    # For normalised gauss curve sigma=1/(amp*sqrt(2*pi))
    if start_point is None:
        off = min(y)  # good enough starting point for offset
        com = x[y.index(max(y))]
        amp = max(y) - off
        sigma = trapz([v-off for v in y], x) / (amp*math.sqrt(2*math.pi))                 
        start_point = [off, amp, com , sigma]                                       
        
    class Model(MultivariateJacobianFunction):
        def value(self, variables):
            value = ArrayRealVector(len(x))
            jacobian = Array2DRowRealMatrix(len(x), 4)        
            for i in range(len(x)):
                (a,b,c,d) = (variables.getEntry(0), variables.getEntry(1), variables.getEntry(2), variables.getEntry(3))   
                v = math.exp(-(math.pow((x[i] - c), 2) / (2 * math.pow(d, 2))))
                model = a + b * v
                value.setEntry(i, model)                                    
                jacobian.setEntry(i, 0, 1)                  # derivative with respect to p0 = a                        
                jacobian.setEntry(i, 1, v)                  # derivative with respect to p1 = b            
                v2 = b*v*((x[i] - c)/math.pow(d, 2))
                jacobian.setEntry(i, 2, v2)                 # derivative with respect to p2 = c        
                jacobian.setEntry(i, 3, v2*(x[i] - c)/d )   # derivative with respect to p3 = d       
            return Pair(value, jacobian)
    
    model = Model()
    target = [v for v in y]      #the target is to have all points at the positios    
    (parameters, residuals, rms, evals, iters) = optimize_least_squares(model, target, start_point, weights)        
    return parameters



start = time.time()
(off, amp, com, sigma) = fit_gaussian_offset(ydata, xdata, None, None)
f = Gaussian(amp, com, sigma)
gauss = [f.value(i)+off for i in xdata]
s = LinePlotSeries("Fit with offset")    
p.addSeries(s)
s.setData(to_array(xdata,'d'), to_array(gauss,'d'))
error=0
for i in range(len(ydata)) : error += abs(ydata[i]-gauss[i])
print "\nFit with offset:\off: ", off , "  amp: ",  amp, "  com: ",  com, "  sigma: ",  sigma, "  error: ",  error
print "Time: ",(time.time()-start)

start = time.time()
(amp, com, sigma) = fit_gaussian(ydata, xdata, None, None)
f = Gaussian(amp, com, sigma)
gauss = [f.value(i) for i in xdata]
s = LinePlotSeries("Normal Fit")    
p.addSeries(s)
s.setData(to_array(xdata,'d'), to_array(gauss,'d'))
error=0
for i in range(len(ydata)) : error += abs(ydata[i]-gauss[i])
print "\nNormal fit:\amp: ",  amp, "  com: ",  com,  sigma, "  error: ",  error, "  time: ",   (time.time()-start)

start = time.time()
[off, amp, com, sigma] = gfitoff(xdata, ydata, off=None, amp=None, com=None, sigma=None)
from mathutils import Gaussian
g = Gaussian(amp, com, sigma)
gauss = [g.value(i)+off for i in xdata]
s = LinePlotSeries("CPython Fit")    
p.addSeries(s)
s.setData(to_array(xdata,'d'), to_array(gauss,'d'))
error=0
for i in range(len(ydata)) : error += abs(ydata[i]-gauss[i])
print "\nCPython Fit:\n", off , "  amp: ",  amp, "  com: ",  com, "  sigma: ",  sigma,  error, "  time: ",   (time.time()-start)





off = min(ydata)  # good enough starting point for offset
com = xdata[ydata.index(max(ydata))]
amp = max(ydata) - off
sigma = trapz([v-off for v in ydata], xdata) / (amp*math.sqrt(2*math.pi))                 
g = Gaussian(amp, com, sigma)
gauss = [g.value(i)+off for i in xdata]
s = LinePlotSeries("Start Point")    
p.addSeries(s)
s.setData(to_array(xdata,'d'), to_array(gauss,'d'))
error=0
for i in range(len(ydata)) : error += abs(ydata[i]-gauss[i])
print "\nStartPoint:\n", off , "  amp: ",  amp, "  com: ",  com, "  sigma: ",  sigma,  error, "  time: ",   (time.time()-start)


fit_point_list = create_fit_point_list(ydata, xdata, None)
(amp, com, sigma) = GaussianCurveFitter.ParameterGuesser(fit_point_list).guess().tolist()           
gauss = [g.value(i)+off for i in xdata]
s = LinePlotSeries("Default SP")    
p.addSeries(s)
s.setData(to_array(xdata,'d'), to_array(gauss,'d'))
error=0
for i in range(len(ydata)) : error += abs(ydata[i]-gauss[i])
print "\Default SP:\n", off , "  amp: ",  amp, "  com: ",  com, "  sigma: ",  sigma,  error, "  time: ",   (time.time()-start)