Positionssytem
==============

Zum Bestimmen der Position mittles Schall wird eine Lösung einer Gleichung 4. Grades verlangt. 
Deswegen gibt es im nächsten Abschnitt einen Solver für Gleichungen 4. Grades.
Gleichungen 5 und höheren Grades werden mit Newtons Tangentenverfahren gelöst.
Das ist nicht immer zuverlässig.

%matplotlib inline

import matplotlib.pyplot as plt
import numpy as np
from math import *

class R:
    def __contains__(self, other):
        return True
    def __eq__(self, other):
        return True
    
R = R()

def solve(coefficients):
    assert coefficients, 'need something to solve'
    order = len(coefficients) - 1
    name = "solve_{}".format(order)
    if not name in globals():
        return solve_n(coefficients)
    function = globals()[name]
    return function(*coefficients)

def get_coefficients(*zeros, multiplier = 1, add = 0):
    l = [1]
    for zero in zeros:
        l = [1] + [l[i] * -zero + l[i + 1] for i in range(len(l) - 1)] + [l[-1] * -zero]
    l = [a * multiplier for a in l]
    l[-1] += add
    return l

assert get_coefficients(5, -2) == [1, -3, -10], "1x² - 3x - 10"

def call(coefficients, x):
    return sum(x ** (len(coefficients) - i - 1) * c for i, c in enumerate(coefficients))

assert call([1, -3, -10], -2) == 0
assert call([1, -3, -10], 5) == 0
assert call([1, -3, -10], 0) != 0

def approximates(a, b, epsilon = 1e-10):
    return abs(a - b) < epsilon

def test_solver(*zeros, epsilon = 1e-10, debug = False):
    if debug: print("------ Test Solver ------")
    coefficients = get_coefficients(*zeros)
    for zero in zeros:
        assert approximates(call(coefficients, zero), 0, epsilon = epsilon), \
               "call({}, {}) == {} == {}".format(coefficients, zero, call(coefficients, zero), 0)
    zeros = list(set(zeros))
    zeros.sort()
    solutions = list(solve(coefficients))
    solutions.sort()
    differences_are_very_small = [approximates(a, b, epsilon = epsilon) for a, b in zip(zeros, solutions)]
    assert len(zeros) == len(solutions),    "{} == {}".format(zeros, solutions)
    assert all(differences_are_very_small), "{} == {}".format(zeros, solutions)

def test_zeros(*zeros, epsilon = 1e-10, expected_number_of_zeros = 1, add = 0, debug = False):
    if debug: print("------ Test Zeros ------")
    c = get_coefficients(*zeros, add = add)
    if debug: 
        print("coefficients: {}".format(c))
    zeros = solve(c)
    if debug: 
        print("zeros: {}".format(zeros))
    assert len(zeros) == expected_number_of_zeros
    for zero in zeros:
        assert approximates(call(c, zero), 0), "f({}) == 0 <=> {} == 0".format(zero, call(c, zero), 0)

def test_polynom(*coefficients, epsilon = 1e-10):
    zeros = list(solve(coefficients))
    zeros.sort()
    for zero in zeros:
        assert approximates(call(coefficients, zero), 0, epsilon = epsilon), \
               "call({}, {}) == {} == {}".format(coefficients, zero, call(coefficients, zero), 0)
    
    

def coefficients_to_string(coefficients):
    if not coefficients:
        return ''
    if len(coefficients) == 1:
        return "{}".format(coefficients[0])
    s = "{} * x + {}".format(coefficients[-2], coefficients[-1])
    for exponent, coefficient in enumerate(reversed(coefficients[:-2]), 2):
        s = s = "{} * x**{} + ".format(coefficient, exponent) + s
    return s

assert coefficients_to_string([3, 4, 5, 6]) == "3 * x**3 + 4 * x**2 + 5 * x + 6", coefficients_to_string([3, 4, 5, 6])

def plot_function(coefficients, from_x = -2, to_x = 4, samples = 2000):
    xs = np.linspace(from_x, to_x, samples)
    plt.plot(xs, [call(coefficients, x )for x in (s / samples * (to_x - from_x) + from_x for s in range(samples))])
    return plt.title("y = f(x) = " + coefficients_to_string(coefficients))

plot_function([1, -12, 3, -8, 5])

def solve_0(a):
    if a == 0:
        return [0]
    return []

test_solver()


def solve_1(a, b):
    "x | ax + b = 0"
    if a == 0:
        return solve_0(b)
    return [- b / a]


test_solver(3)
test_solver(-2)

def solve_2(a, b, c):
    if a == 0:
        return solve_1(b, c)
    p = b / a
    q = c / a
    D = p * p / 4 - q
    if D < 0:
        return []
    if D == 0:
        return [-p / 2]
    return [- p / 2 + D ** 0.5, - p / 2 - D ** 0.5]

test_solver(3, 2.3)
test_solver(-2, 100)

def differenciate(coefficients):
    return [c * (len(coefficients) - i) for i, c in enumerate(coefficients[:-1])]


def newton(coefficients, epsilon = 1e-15, max_iterations = 1000000, start = 1):
    differenciation = differenciate(coefficients)
    differenciation2 = differenciate(differenciation)
    zero = start
    last_zero = None
    for iteration in range(1, max_iterations + 1):
        y = call(coefficients, zero)
        if abs(y) < epsilon:
            break
        last_zero = zero
        
        slope = call(differenciation, zero)
        #print("slope:", slope)
        # optimized newton
        # http://www.tavrodir.lima-city.de/Arbeiten/Newton-%20und%20Sekantenverfahren%20Beleg.pdf
        assert slope
        slope2 = call(differenciation2, zero)
        #print("slope2:", slope2)
        #print("x:", zero)
        zero -= y / slope / (1 - y * slope2 / slope / slope)
        
        if last_zero == zero:
            break
    
    print(iteration)
    if iteration == max_iterations:
        raise ValueError("Could not complete newton({}, epsilon = {}, max_iterations = {}) at x = {} f(x) = {}"\
                         .format(coefficients, epsilon, max_iterations, zero, call(coefficients, zero)))
    return zero

def test_newton(*zeros, epsilon = 1e-15):
    zero = newton(get_coefficients(*zeros), epsilon = epsilon)
    assert any(approximates(zero, x) for x in zeros), "{} in {}".format(zero, zeros)


#test_newton(2)
#test_newton(-5, 2)
#test_newton(10.3, -0.0001, 2)
#test_newton(-33, 100, 2, -3, epsilon = 0.000001)
test_newton(21, 22, 23)

c = get_coefficients(88, 8, 888)
newton(c, start = 887.9993)

def solve_3_newton(p, q, r, s):
    if p == 0:
        return solve_2(q, r, s)
    zero = newton([p, q, r, s])
    a = p
    b = q + p * zero
    c = - s / zero
    assert approximates(c - b * zero, r), "r = {} = c - bz = {} - {} = {}".format(r, c, b * zero, c - b * zero)
    return [zero] + solve([a, b, c])

def curt(x):
    "cubic root"
    if x == 0:
        return 0
    sign = abs(x) / x
    x = abs(x)
    return (x ** (1./3.)) * sign

def solve_3(A, B, C, D):
    if A == 0:
        return solve_2(B, C, D)
    # x³ + ax² + bx + c = 0
    a = B / A
    b = C / A
    c = D / A
    
    # x = z - a / 3
    # z³ + pz + q = 0
    p = b - a*a / 3
    q = 2 * a*a*a / 27 - a * b / 3 + c
    
    D = q*q / 4 + p*p*p / 27
    if D > 0:
        #print("{3} -> D > 0")
        #print("D: {}".format(D))
        #print("sqrt_of_D: {}".format(sqrt(D)))
        #print("minus_q_half: {}".format(- q / 2))
        u = curt(- q / 2 + sqrt(D))
        #print("u: {}".format(u))
        v = curt(- q / 2 - sqrt(D))
        #print("v: {}".format(v))
        z = u + v
        #print("z: {}".format(z))
        return [z - a/3]
    elif p == 0 and q == 0:
#        print("{3} -> p == 0 and q == 0")
        # z = 0
        return [- a/3]
    
#    elif p == 0: # and q != 0
#        # impossible case since it implies D < 0
#        print("-> p == 0")
#        z = (-q) ** (1.0 / 3.0)
#        return [z - a/3]
    elif D == 0: # and (p != 0 or q != 0)
#        print("{3} -> D == 0 and (p != 0 or q != 0)")
        u = curt(- q / 2)
        z1 = 2 * u
        z23 = - u
        return [z1 - a/3, z23 - a/3]
    else: # D < 0
#        print("{3} -> D < 0")
        #print("a: {}".format(a))
        #print("b: {}".format(b))
        #print("c: {}".format(c))
        #print("p: {}".format(p))
        #print("q: {}".format(q))
        #print("sq3p: {}".format(sqrt(-3 / p)))
        m = sqrt(-4/3 * p)
        #print("m: {}".format(m))
        n = 1 / 3 * acos(-q/2 * sqrt(-27 / p/p/p))
        #print("n: {}".format(n))
        z2 = -m * cos(n + pi / 3)
        z1 =  m * cos(n)
        z3 = -m * cos(n - pi / 3)
        #print("z2: {}".format(z2))
        #print("z1: {}".format(z1))
        #print("z3: {}".format(z3))
        assert z1.imag == 0
        assert z2.imag == 0
        assert z3.imag == 0
        return [z1 - a/3, z2 - a/3, z3 - a/3]

if 1:
    print("       D > 0 expected")
    test_zeros(1, 2, 3, add = 100)
    print("       p == 0 and q == 0 expected")
    test_solver(0,0,0)
    print("       D == 0 and (p != 0 or q != 0) expected")
    test_zeros(0,0,3,expected_number_of_zeros = 2)

    print("       D < 0 expected")
    test_solver(1,2,3)
    test_solver(3, 5, -2222)
    test_solver(0.0001,-10.12333,0)
    test_solver(-10,-10.12333,0)
    test_solver(-10,0.0001,0)
    test_solver(-10,0.0001,-10.12333)

solve_3(1, 0, 1, 30)

# 1.00*x^3 + -380.00*x^2 + 22647.00*x + -369468.00
#solve_3(1, -380, 22647, -369468)
32.999996185302734 * 0.0000001 -33


def sqrt_2(a):
    if a == 0:
        return 0
    b = 1
    last_b = 0
    while last_b != b:
        last_b = b
        b = (b + a / b) / 2
    return b

assert approximates(sqrt_2(2), sqrt(2)), "{} == {} <=> sqrt_2(2) == sqrt(2)".format(sqrt_2(2), sqrt(2))

def curt_2(a):
    if a == 0:
        return 0
    b = a
    last_b_1 = 0;
    last_b_2 = 0;
    while (last_b_1 != b and last_b_2 != b):
        last_b_1 = b;
        b = (b + a / b / b) / 2;
        last_b_2 = b;
        b = (b + a / b / b) / 2;
    return b

assert approximates(curt_2(2), curt(2)), "{} == {} <=> sqrt_2(2) == sqrt(2)".format(curt_2(2), curt(2))


def sqrt_2(a):
    if a == 0:
        return 0
    b = 1
    for i in range(10):
        b = (b + a / b) / 2
    return b

sqrt_2(225.03703703703704), sqrt(225.03703703703704)

def curt_3(a):
    if a == 0:
        return 0
    b = 1
    for i in range(50):
        b = (b + a / b / b) / 2;
    return b
curt_3(15.001234517100153), 15.001234517100153** (1/3)

def solve_4(e, f, g, h, i):
    if e == 0:
        return solve_3(f, g, h, i)
    #print("Solving: {} = 0".format(coefficients_to_string([e, f, g, h, i])))
    a = f / e
    b = g / e
    c = h / e
    d = i / e
    #print("Solving: {} = 0".format(coefficients_to_string([1, a, b, c, d])))
    
    # x = z - a/4
    p = b - 3 / 8 * a*a
    q = a*a*a / 8 - a * b / 2 + c
    r = -3/256 * a*a*a*a + a*a * b / 16 - a * c / 4 + d
    #print(-3/256 * a*a*a*a)
    #print(-3/256 * a*a*a*a + a*a * b / 16 )
    #print(-3/256 * a*a*a*a + a*a * b / 16 - a * c / 4)
    #print(-3/256 * a*a*a*a + a*a * b / 16 - a * c / 4 + d)
    #print("p: {} q: {} r: {}".format(p, q, r))
    
    if approximates(p, 0) and approximates(q, 0):
#        print("{4} p == 0 and q == 0")
        if approximates(r, 0):
            return [0]
        elif r > 0:
            return []
        return (-r) ** 0.5
    elif approximates(q, 0):
#        print("{4} p != 0 and q == 0")
        # v = z²
        vs = solve([1, p, r])
        xs = []
        #print('vs: {}'.format(vs))
        for v in vs:
            if approximates(v, 0):
                v = 0
            elif v < 0:
                continue
            z1 = v ** 0.5
            z2 = -z1
            xs.append(z1 - a / 4)
            xs.append(z2 - a / 4)
        return xs
    #print("{{4}} solve({})".format([8, -4*p, -8*r, 4 * p * r - q*q]))
    Ps = solve([8, -4*p, -8*r, 4 * p * r - q*q])
    #print("Ps: {}".format(Ps))
    has_solution = False
    for P in Ps:
        Q_squared = 2 * P - p
        R_squared = P * P - r
        #print("{{4}} P: {} Q_squared: {} R_squared: {}".format(P, Q_squared, R_squared))
        if approximates(Q_squared, 0):
            Q_squared = 0
        elif Q_squared < 0:
            continue
        if approximates(R_squared, 0):
            R_squared = 0
        elif R_squared < 0:
            continue
        Q1 = sqrt(Q_squared)
        Q2 = -Q1
        R1 = sqrt(R_squared)
        R2 = -R1
        for R, Q in ((R1, Q1), (R1, Q2), (R2, Q1), (R2, Q2)):
#            print("{{4}} P: {} approximately 0: {} | {}".format(P, Q * R + -q / 2, Q * R - -q / 2))
            if approximates(Q * R, -q / 2, epsilon = 1e-9):
#                print("approximation of 0: {}".format(Q * R +q / 2))
                has_solution = True
                break
        break
    if not has_solution:
        #print('{4} no solution found')
        return []
    zs = solve([1, Q, P + R]) + solve([1, -Q, P - R])
    # x = z - a/4
    return [z - a / 4 for z in zs]

test_solver(40,42,43,44)


test_solver(1,2,3,4)
test_solver(0.018466806307818034, -0.02575054181030545, -18.59653702522963, -19.600040761851144)
test_solver(0.002078698483378716, -0.013961191268707651, -18.91277956509588, -19.279199464702053)
test_solver(--10.12333, --10, -0, -0.01, epsilon = 1e-6)
test_solver(-10.12333, -10, 0, 0.0001, epsilon = 1e-6)


def solve_n(coefficients):
    "does not work if newton failes"
    if coefficients[0] == 0:
        return solve(coefficients[1:])
    zero = newton(coefficients)
    if zero == 0:
        assert coefficients[-1] == 0
        return [0] + solve(coefficients[:-1])
    less_coefficients = [coefficients[0]]
    for coefficient in coefficients[1:-1]:
        less_coefficients.append(coefficient + zero * less_coefficients[-1])
    assert approximates(less_coefficients[-1], -coefficients[-1] / zero), \
           "{} = {} = {} / z | z = {}".format(less_coefficients[-1], -coefficients[-1] / zero, -coefficients[-1], zero)
    return [zero] + solve(less_coefficients)

test_solver(1, 2, 3, 4, 5)
test_solver(0, 1, 2, 3, 4, 5)
#test_solver(70, 5, 30, 1, -1, -8) # should choose another epsilon

def get_dt(a, b, x, y):
    u = sqrt(x*x + y*y)
    w = sqrt(y*y + (b - x)*(b - x))
    v = sqrt((a + y)*(a + y) + x*x)
    
    #print("{{p}} u, v, w = {}, {}, {}".format(u, v, w))
    
    g1 = v - u
    g2 = v - w
    g3 = u - w
    
    return g1, g2, g3

def check(dt1, dt2, a, b, x, y):
    "=> the error"
    dt3 = dt2 - dt1
    
    g1, g2, g3 = get_dt(a, b, x, y)

#    if g1 == dt1 and g2 == dt2: print('{p} g1 == dt1 and g2 == dt2')
    error = (g1 - dt1)**2 + (g2 - dt2)**2 + (g3 - dt3)**2
    return error

def position(dt1, dt2, a, b):
    """
    dt1 = PB - CP
    dt2 = BP - AP
    """
    g1 = dt1
    print("g1: {}".format(g1))
    g2 = dt2
    print("g2: {}".format(g2))
    g3 = g2 - g1 # CP - AP    
    print("g3: {}".format(g3))
    
    n =  4 * g1*g1
    o =  4 * a*a
    p =  4 * a*a*a - 4 * a * g1*g1
    q = a**4 - 2 * a*a * g1*g1 + g1**4
    print("n: {}".format(n))
    print("p: {}".format(p))
    print("q: {}".format(q))
    
    r =  4 * g3*g3
    s =  4 * b*b
    t = -4 * b**3 + 4 * b * g3*g3
    v = b**4 - 2 * b*b * g3*g3 + g3**4
    print("r: {}".format(r))
    print("s: {}".format(s))
    print("t: {}".format(t))
    print("v: {}".format(v))
    
    d = o - n
    e = s - r
    
    print("d: {}".format(d))
    print("e: {}".format(e))
    
    Ps = []
    if r == 0:
#        print("{p} r == 0")
        xs = solve([e, t, v])
        for x in xs:
            ys = solve([d, p, q - n * x*x])
            Ps.extend([(x, y) for y in ys])
    elif n == 0:
        print("{p} n == 0")
        print("solving {} {} {}".format(d, p, q))
        ys = solve([d, p, q])
        for y in ys:
            print("solving {} {} {}".format(e, t, v - r * y*y))
            xs = solve([e, t, v - r * y*y])
            Ps.extend([(x, y) for x in xs])
    else:
        F = -2 * e * r
        G =  2 * e * v
        H = e*e / n/n
        K = F / n
        L = G / n
        M = t*t / n

        A = H * d*d + K * d + r*r
        B = 2 * H * d * p + K * p
        C = 2 * H * d * q + H * p*p + K * q + L * d - 2 * r * v - M * d
        D = 2 * H * p * q + L * p - M * p
        E = H * q*q + L * q + v*v - M * q
        
        print("A:", A);
        print("B:", B);
        print("C:", C);
        print("D:", D);
        print("E:", E);
        
        ys = solve([A, B, C, D, E])
#        print("{{p}} ys: {} == solve({})".format(ys, [A, B, C, D, E]))
        for y in ys:
            xs = solve([e, t, v - r * y*y])
#            print("{{p}} y: {} xs: {}".format(y, xs))
            Ps.extend([(x, y) for x in xs])
    # check the results
    choose = lambda xy: check(dt1, dt2, a, b, xy[0], xy[1])
    print("{{p}} PS: {}".format("\n{p}     ".join(map(lambda xy: "{}\t{}\t{}".format(xy[0], xy[1], choose(xy)), Ps))))
    x, y = min(Ps, key = choose)
    return x, y
    
def assert_approximates(a, b, epsilon = 1e-15):
    assert approximates(a, b), "{} == {} but difference is {}".format(a, b, a-b)

def test_position(x0, y0, a = 1, b = 2, debug = False):
    if debug: print("------ Test x: {} y: {} ------".format(x0, y0))
    dt1, dt2, dt3 = get_dt(a, b, x0, y0)
    if debug: print("{{test}} dt1, dt2, dt3 = {}, {}, {}".format(dt1, dt2, dt3))
    x, y = position(dt1, dt2, a, b)
    if debug: print("{{test}} x0: {} y0: {} \n{{test}}  x: {}  y: {}".format(x0, y0, x, y))
    assert approximates(x, x0, epsilon = 1e-8)
    assert approximates(y, y0, epsilon = 1e-8)

if 0:
    # r != 0 and n != 0
    test_position(2, -1)

    test_position(100, -12)
    test_position(123.12312321321, -100)
    test_position(-333.213, -12)

    # r == 0
    test_position(1, 1)
    test_position(1, 0)
    test_position(1, -1)
    test_position(0, 0)
    test_position(1, -0.5)
    # n == 0
    test_position(12, -0.5)

test_position(-333.213, -12, 20, 10)

l = [0.0332622528, -0.0665245056, -3.991614818]

solve(l)

call(differenciate(l), 1)

%matplotlib inline

import matplotlib.pyplot as plt
import numpy as np
from math import *

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d, Axes3D #<-- Note the capitalization! 
fig = plt.figure()

ax = Axes3D(fig) #<-- Note the difference from your original code...

X = np.linspace(-5, 5, 200)
Y = np.linspace(-5, 5, 300)
X, Y = np.meshgrid(X, Y)
Z = X**2 + Y** 2
cset = ax.plot_surface(X, Y, Z,cmap=plt.cm.coolwarm, linewidth=0, antialiased=False)
ax.clabel(cset, fontsize=9, inline=1)
plt.show()

fig, ax = plt.subplots()

p = ax.pcolor(X/(2*pi), Y/(2*pi), Z, cmap=plt.cm.RdBu, vmin=abs(Z).min(), vmax=abs(Z).max())
cb = fig.colorbar(p, ax=ax)


def get_dt(a, b, x, y):
    u = sqrt(x*x + y*y)
    w = sqrt(y*y + (b - x)*(b - x))
    v = sqrt((a + y)*(a + y) + x*x)
    
    #print("{{p}} u, v, w = {}, {}, {}".format(u, v, w))
    
    g1 = v - u
    g2 = v - w
    g3 = u - w
    
    return g1, g2, g3

def check(dt1, dt2, a, b, x, y):
    "=> the error"
    dt3 = dt2 - dt1
    
    g1, g2, g3 = get_dt(a, b, x, y)

#    if g1 == dt1 and g2 == dt2: print('{p} g1 == dt1 and g2 == dt2')
    error = abs(g1 - dt1) #+ (g2 - dt2)**2 + (g3 - dt3)**2
    return error

a = 2
b = 1
x0 = -3
y0 = -3
dt1, dt2, dt3 = get_dt(a, b, x0, y0)

X = np.linspace(-30, 30, 200)
Y = np.linspace(-30, 30, 200)
Z = [[check(dt1, dt2, a, b, x, y) for x in X] for y in Y]

fig = plt.figure()
ax = Axes3D(fig) #<-- Note the difference from your original code...

X, Y = np.meshgrid(X, Y)
cset = ax.plot_surface(X, Y, Z,cmap=plt.cm.coolwarm, linewidth=0, antialiased=False)
plt.show()


def get_dt(a, b, x, y):
    u = sqrt(x*x + y*y)
    w = sqrt(y*y + (b - x)**2)
    #v = sqrt(y*y + (a + x)**2) # in straight line
    v = sqrt((a + y)*(a + y) + x*x) # rectangular
    
    #print("{{p}} u, v, w = {}, {}, {}".format(u, v, w))
    
    g1 = v - u
    g2 = v - w
    g3 = u - w
    
    return g1, g2, g3

def check(dt1, dt2, a, b, x, y):
    "=> the error"
    dt3 = dt2 - dt1
    
    g1, g2, g3 = get_dt(a, b, x, y)

#    if g1 == dt1 and g2 == dt2: print('{p} g1 == dt1 and g2 == dt2')
    error = abs(g1 - dt1)**2 + abs(g2 - dt2)**2# + abs(dt3 - g3)
    return (error)

def plot_dts(x0, y0, a, b, dim = 30):
    dt1, dt2, dt3 = get_dt(a, b, x0, y0)

    X = np.linspace(-dim, dim, 500)
    Y = np.linspace(dim, -dim, 500)
    Z = [[check(dt1, dt2, a, b, x, y) for x in X] for y in Y]

    fig = plt.figure()
    ax = Axes3D(fig) #<-- Note the difference from your original code...

    X, Y = np.meshgrid(X, Y)
    cset = ax.plot_surface(X, Y, Z,cmap=plt.cm.coolwarm, linewidth=0, antialiased=False)
    ax.view_init(50, 200)
    plt.show()

plot_dts(-3, -3, 2, 1, 10)

plot_dts(5, 5, 4, 4)

from math import sqrt

def get_dt(x, y, p):
    return sqrt((x - p[0])**2 + (y - p[1]) ** 2)

def get_dts(x, y, Ps):
    A, B, C, D = Ps
    AP = get_dt(x, y, A)
    BP = get_dt(x, y, B)
    CP = get_dt(x, y, C)
    DP = get_dt(x, y, D)
    return (BP - CP), (BP - AP), (BP - DP)

def error_f(x):
    return x * x

error_f = abs

use_4_points = True
def get_error(x, y, dts, Ps):
    dts2 = get_dts(x, y, Ps)
    return error_f(dts2[0] - dts[0]) + error_f(dts2[1] - dts[1]) + error_f((dts2[2] - dts[2])) * use_4_points

def plot_dts(x0, y0, Ps, dim = 30, middle = None):
    dts = get_dts(x0, y0, Ps)
    if middle is None:
        middle = x0, y0

    X = np.linspace(-dim + middle[0], dim + middle[0], 100)
    Y = np.linspace(middle[1] - dim, middle[1] + dim, 100)
    Z = [[get_error(x, y, dts, Ps) for x in X] for y in Y]

    X, Y = np.meshgrid(X, Y)
    Z = np.array(Z)
    fig, ax = plt.subplots()
    p = ax.pcolor(X, Y, Z, cmap=plt.cm.RdBu, vmin=min(map(min, Z)), vmax=max(map(max, Z)))
    cb = fig.colorbar(p, ax=ax)

#plot_dts(0, 0.75, [(0,0), (0,1), (1,0)], 3)
use_4_points = True
plot_dts(12, -.5, [(1,0), (0,2),(0,0), (1, 2) ], 30)

use_4_points = False
plot_dts(2, -1, [(1,0), (0,2),(0,0), (1, 2) ], 2)


plot_dts(0.5, 0.5, [(1,0), (0,2),(0,0), (1, 2) ],2)

plot_dts(12, -.5, [(1,0), (0,2),(0,0), (1, 2) ], 20)

def reflect(x, x0, m):
    return [x0[i] + m * (x0[i] - x[i]) for i in range(len(x))]

def minimize(function, points, alpha = 1, gamma = 2, peta = -0.5, delta = 0.5, error = 1e-15):
    # Nelder Mead in Python
    # https://en.wikipedia.org/wiki/Nelder%E2%80%93Mead_method
    dim = len(points[0])
    x0 = None
    ii = 0
    for i in range(200):
        ii += 1
        # 1
        values = [(function(point), point) for point in points]
        values.sort()
        values, points = zip(*values)
        points = list(points)
        print(points)
        print('error:', values)
        # 2
        x0 = [sum([point[i] for point in points[:-1]]) / (len(points) - 1) for i in range(dim)]
        if values[0] < error:
            break
        print(" ----- x0:", x0, "-----")
        # 3 reflection
        xr = reflect(points[-1], x0, alpha)
        vr = function(xr)
        print("xr:", xr, vr)
        if vr < values[-2] and vr >= values[0]:
            points[-1] = xr
            print('#reflection')
            continue
        # 4 expansion
        if values[0]> vr:
            xe = reflect(points[-1], x0, gamma)
            ve = function(xe)
            print("xe:", xe, ve)
            print('#expansion')
            if ve < vr:
                points[-1] = xe
                continue
            else:
                points[-1] = xr
                continue
        # 5 contraction
        assert vr >= values[-2]
        xc = reflect(points[-1], x0, peta)
        vc = function(xc)
        print("xc:", xc, vc)
        if vc < vr:
            print("#contraction")
            points[-1] = xc
            continue
        # 6 reduction
        points[1:] = [reflect(points[i], points[0], delta) for i in range(1, len(points))]
        print("#reduction")
        continue
    print('iterations:', ii)
    return x0

def minimization_function(dts, Ps):
    return lambda p: get_error(p[0], p[1], dts, Ps)

def minimization_function_form_xy(x0, y0, Ps):
    dts = get_dts(x0, y0, Ps)
    print("---- minimize ----")
    return minimization_function(dts, Ps)

middle_influence_on_starting_points = 0.99

def mix(v1, v2, v3):
    middle = (v1 + v2 + v3) / 3
    return v1 * (1 - middle_influence_on_starting_points) + middle * middle_influence_on_starting_points

def mixPoint(P1, P2, P3):
    return mix(P1[0], P2[0], P3[0]), mix(P1[1], P2[1], P3[1]), 

def test_minimize(x0, y0, Ps):
    starting_points = [mixPoint(Ps[0], Ps[1], Ps[2]), mixPoint(Ps[1], Ps[0], Ps[2]), mixPoint(Ps[2], Ps[1], Ps[0])]
    return minimize(minimization_function_form_xy(x0, y0, Ps), starting_points)

_error_f = error_f
error_f = lambda x: x*x
#test_minimize(12, -.5, [(1,0), (0,2),(0,0), (1,2)])
test_minimize(2, -1, [(1,0), (0,2),(0,0), (1,2) ])

#test_minimize(1, 0.0000001, [(1,0), (0,2),(0,0), (1,2) ])

error_f = _error_f