'''
oppgave 2.7_5 see p. 136 in T.Sauer, Numerical Analysis
'''

import numpy as np

centers = np.array([[1,1,0],[1,0,1],[0,1,1]],dtype='f8')
radii   = np.array([1,1,1],dtype='f8')

def F(x):
    '''
    Compute the residual of the equation system
    '''
    n = len(radii)
    r = np.zeros(n)
    for i in range(n):
        r[i] = np.linalg.norm(x-centers[i],2)**2 - radii[i]**2
    return r

def DF(x):
    '''
    Compute the Jacobian of the equation system
    '''
    n = len(radii)
    J = np.zeros((n,n))
    for i in range(n):
        J[i,:] = 2*(x-centers[i,:])
    return J

if __name__=='__main__':
    NMax = 100
    tol  = 1.0E-10

    #x = np.array([1.5,1.5,1.5])
    x = np.array([.5,.5,.5])

    for i in range(NMax):
        r = F(x)
        print('Iteration: %5d, residual: %e' % (i,np.linalg.norm(r,2)))
        print('x = ', x)
        if np.linalg.norm(r,2) < tol:
            print('Success!')
            break
        J = DF(x)
        x = x - np.linalg.solve(J,r)