#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Tue Sep 26 08:13:07 2023

@author: laurentdumas
"""

import numpy as np 
(x1,y1,z1)=(-11716.227778, -10118.754628, 21741.083973)
(x2,y2,z2)=(-12082.643974,-20428.242179,11741.374154)
(x3,y3,z3)=(14373.286650,-10448.439349,19596.404858)
d1=22163.847742
d2=21492.777482
d3=21492.469326
def F(x,y,z):
    f1=(x-x1)**2+(y-y1)**2+(z-z1)**2-d1**2 
    f2=(x-x2)**2+(y-y2)**2+(z-z2)**2-d2**2
    f3=(x-x3)**2+(y-y3)**2+(z-z3)**2-d3**2
    return np.array([f1,f2,f3])
def Df(x,y,z):
    l11,l12,l13=(2*(x-x1),2*(y-y1),2*(z-z1))
    l21,l22,l23=(2*(x-x2),2*(y-y2),2*(z-y2))
    l31,l32,l33=(2*(x-x3),2*(y-y3),2*(z-z3))
    return np.array([[l11,l12,l13],[l21,l22,l23],[l31,l32,l33]])
A0=(0,0,0)
for n in range(15):
    A0=A0-np.linalg.inv(Df(A0[0],A0[1],A0[2])).dot(F(A0[0],A0[1],A0[2]))
    print(A0[0],A0[1],A0[2])
    
r=((A0[0])**2+(A0[1])**2+(A0[2])**2)**(0.5)
print(r)