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

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 SSR error
            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.solve(A, root + b)
            solutions.append(np.array([X,Y]))

        print(max(solutions, key = ssr_error)[:2])

        return max(solutions, key = ssr_error)

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

c = 299792

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

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