import numpy as np

from mdsmap_sim.lateration import circular_lateration, square_lateration


def test_lateration_noise_free():
    anchors = np.array([[0, 0], [10, 0], [0, 10], [10, 10]], dtype=float)
    target = np.array([4.0, 6.0])
    distances = np.linalg.norm(anchors - target, axis=1)
    assert np.linalg.norm(circular_lateration(anchors, np.arange(4), distances) - target) < 1e-7
    estimate = square_lateration(anchors, np.arange(4), distances)
    assert np.all(np.isfinite(estimate))
    assert np.linalg.norm(estimate - target) < 1.0