from dataclasses import dataclass
from random import randrange
import math
import numpy as np

M = np.diag([1, 1, -1]) 

@dataclass
class Vertexer:

    nodes: np.ndarray
    c: float

    def reconstruct(self, times):

        A = np.append(self.nodes, np.reshape(times, (-1, 1)) * self.c, axis=1)

        def ssr_error(point):
            return np.sum(((np.linalg.norm(self.nodes - point, axis=1) / self.c)  
                   - times)**2)

        def lorentz(a, b):
            # Return Lorentzian Inner-Product
            return np.sum(a * (b @ M), axis=-1)

        b = lorentz(A, A) * 0.5
        C = np.linalg.solve(A, np.ones(3))
        D = np.linalg.solve(A, b)

        roots = np.roots([lorentz(C, C),
                         (lorentz(C, D) - 1) * 2, 
                          lorentz(D, D)])

        solutions = []
        for root in roots:
            X, Y, T = M @ np.linalg.lstsq(A, root + b, rcond=None)[0]
            solutions.append(np.array([X,Y]))

        return(min(solutions, key = ssr_error))

nodes = np.random.random(size=(3,2)) * 100
source = np.random.random(size=(1,2)) * 100

c = 299792

times = np.sum(np.sqrt((nodes - source)**2), axis=1) / c

print('Actual:', source)
myVertexer = Vertexer(nodes, c)
print(myVertexer.reconstruct(times))