"""
Function fitting and peak search directly with org.apache.commons.math3
""" 

import org.apache.commons.math3.util.FastMath as FastMath

import org.apache.commons.math3.stat.regression.SimpleRegression as SimpleRegression

import org.apache.commons.math3.analysis.polynomials.PolynomialFunction as PolynomialFunction
import org.apache.commons.math3.analysis.function.Gaussian as Gaussian
import org.apache.commons.math3.analysis.function.HarmonicOscillator as HarmonicOscillator 
import org.apache.commons.math3.analysis.solvers.LaguerreSolver as LaguerreSolver

import org.apache.commons.math3.fitting.GaussianCurveFitter as GaussianCurveFitter
import org.apache.commons.math3.fitting.PolynomialCurveFitter as PolynomialCurveFitter
import org.apache.commons.math3.fitting.HarmonicCurveFitter as HarmonicCurveFitter
import org.apache.commons.math3.fitting.WeightedObservedPoint as WeightedObservedPoint

import org.apache.commons.math3.analysis.differentiation.FiniteDifferencesDifferentiator as FiniteDifferencesDifferentiator
import org.apache.commons.math3.analysis.differentiation.DerivativeStructure as DerivativeStructure

 
start = -0
end = 2
step_size = 0.1

result= lscan(inp,(sin,out),start,end,[step_size,],0.1)

readable = result.getReadable(0)
positions = result.getPositions(0)

simple_regression= SimpleRegression()

values = []
for record in result.records:
    simple_regression.addData(record.positions[0], record.values[0])
    values.append(WeightedObservedPoint(1, record.positions[0], record.values[0]))

order = 6
#polynomial_fitter =  PolynomialCurveFitter.create(0).withStartPoint([ 5.0, 5.0, 5.0])
polynomial_fitter = PolynomialCurveFitter.create(order)
gaussian_fitter = GaussianCurveFitter.create();
#harmonic_fitter = HarmonicCurveFitter.create();


best = polynomial_fitter.fit(values)
fitted_polynomial_function = PolynomialFunction(best)


(normalization, mean, sigma) = gaussian_fitter.fit(values)
print (normalization, mean, sigma) 
fitted_gaussian_function = Gaussian(normalization, mean, sigma)
print "Mean: " + str(mean)

#(amplitude, angular_frequency, phase) = harmonic_fitter.fit(values)
#print (amplitude, angular_frequency, phase) 
#fitted_harmonic_function = HarmonicOscillator(amplitude, angular_frequency, phase)


#differentiator = FiniteDifferencesDifferentiator(order+2, 0.25) #points, step size
#derivative = differentiator.differentiate(fitted_polynomial)
derivative = fitted_polynomial_function.derivative() 
print fitted_polynomial_function.coefficients 
print derivative.coefficients 


regression = []
fit_polinomial = []
fit_gaussian = []
#fit_harmonic = []
fit_polinomial_derivative = []
resolution = step_size/100
for x in frange(start,end,resolution, True):
    regression.append(simple_regression.predict(x))
    fit_polinomial.append(fitted_polynomial_function.value(x))
    fit_gaussian.append(fitted_gaussian_function.value(x))
    #fit_harmonic.append(fitted_harmonic_function.value(x))
    fit_polinomial_derivative.append(derivative.value(x))
x = frange(start, end+resolution, resolution)




def calculate_peaks(function, start_value, end_value , positive=True):
    derivative = function.derivative() 
    derivative2 = derivative.derivative() 
    ret = []
    solver = LaguerreSolver()
    for complex in solver.solveAllComplex(derivative.coefficients, start_value):
        r = complex.real
        if start_value < r < end_value:
            if (positive and (derivative2.value(r) < 0)) or ( (not positive) and (derivative2.value(r) > 0)):         
                ret.append(r)
    return ret

peaks = calculate_peaks(fitted_polynomial_function, start, end)


plots = plot([readable, regression, fit_polinomial, fit_gaussian,  fit_polinomial_derivative] , 
    ["sin", "regression", "fit polinomial", "fit gaussian", "fit harmonic ", "fit polinomial derivative"], 
    xdata = [positions,x,x,x,x,x], title="Data" )


for p in peaks:    
    print "Max: " + str(p)
    plots[0].addMarker(p, None, "Max", None)

#plots[0].addMarker(mean, None, "Gaussian Mean", None)



print result