from mathutils import *

#Fiting center of circle of radius 70  to observerd points
radius = 70.0
x = [30.0,  50.0, 110.0, 35.0, 45.0]
y = [68.0,  -6.0, -20.0, 15.0, 97.0]
p = plot(y, xdata=x)[0]
num_samples = len(x)
w = [ 1.0] * num_samples
w = [0.1,  0.1, 1.0, 0.1, 1.0]
#I = MatrixUtils.createRealIdentityMatrix(len(w))
W = MatrixUtils.createRealDiagonalMatrix(w)


  # the model function components are the distances to current estimated center,
  # they should be as close as possible to the specified radius
class Model(MultivariateJacobianFunction):
    def value(self, variables):
        (cx,cy) = (variables.getEntry(0), variables.getEntry(1))  
        value = ArrayRealVector(num_samples)
        jacobian = Array2DRowRealMatrix(num_samples, 2)
        for i in range(num_samples):
            model = math.hypot(cx-x[i], cy-y[i]) 
            value.setEntry(i, model)
            # derivative with respect to p0 = x center
            jacobian.setEntry(i, 0, (cx - x[i]) / model)
            # derivative with respect to p1 = y center
            jacobian.setEntry(i, 1, (cy - y[i]) / model)
        return Pair(value, jacobian)

model = Model()

# the target is to have all points at the specified radius from the center
target = [radius,] * num_samples

#least squares problem to solve : modeled radius should be close to target radius
initial = [0.0, 0.0 ]
problem = LeastSquaresBuilder().start(initial).model(model).target(target).lazyEvaluation(False).maxEvaluations(1000).maxIterations(1000).weight(W).build()
optimizer = LevenbergMarquardtOptimizer()
optimum = optimizer.optimize(problem)
cx, cy = optimum.getPoint().getEntry(0), optimum.getPoint().getEntry(1)

print "fitted center x: ", cx
print "fitted center y: ", cy
print ""
print "RMS: "          , optimum.getRMS()
print "evaluations: "  , optimum.getEvaluations()
print "iterations: "   , optimum.getIterations()

from plotutils import *

plot_point(p, cx, cy, name="Fit Cente")
plot_circle(p, cx, cy, radius, name="Fit")