"""Unit tests for the LDS-02 adapter (no ROS):  python3 tests/test_adapter.py"""
import math
import os
import sys

import numpy as np

sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
from lds02 import ScanCleaner, lds02_corrupt, sample_params  # noqa: E402
from lds02_adapter import FRONT_IDX, RAY_ANGLES, LidarAdapter, bin_scan  # noqa: E402


def scene(angles):
    """Ranges seen from the origin: wall at 2 m everywhere, a 0.5 m object straight ahead (+-10 deg), one at 1 m on
    the left (80..100 deg)."""
    a = np.mod(angles, 2 * math.pi)
    r = np.full(len(a), 2.0)
    r[(a < math.radians(10)) | (a > math.radians(350))] = 0.5
    r[(a > math.radians(80)) & (a < math.radians(100))] = 1.0
    return r


def test_layouts():
    for n, amin, sign in ((230, 0.0, 1), (360, 0.0, 1), (460, 0.0, 1), (360, -math.pi, 1), (360, 0.0, -1), (455, 1.0, -1)):
        inc = sign * 2 * math.pi / n
        ang = amin + np.arange(n) * inc
        out, seen = bin_scan(scene(ang), amin, inc)
        ref = scene(RAY_ANGLES)
        ok = np.mean(np.abs(out - ref) < 1e-6)
        assert ok > 0.97, (n, amin, sign, ok)                       # only edge rays may differ by a bin
        assert out[0] == 0.5 and abs(out[90] - 1.0) < 1e-6 and out[180] == 2.0, (n, amin, sign)
    print('layouts ok (230 / 360 / 460 points, offset start, clockwise)')


def test_invalid_and_hold():
    c = ScanCleaner()
    s1 = np.full(360, 2.0); s1[40:45] = 0.25
    s2 = np.full(360, 2.0); s2[40:45] = 0.0; s2[300] = np.inf
    c(s1, 0.0)
    o, held = c(s2, 0.2)
    assert abs(o[42] - 0.25) < 1e-6 and o[300] == 3.5 and held == 5
    o, _ = c(np.zeros(360), 1.0)
    assert (o == 3.5).all()
    print('invalid readings / near-field hold ok')


def test_sim_equivalence():
    """The simulator path (Gazebo 360 rays -> lds02_corrupt -> ScanCleaner, front beams by FRONT_GRID) and the robot
    path (LaserScan -> bin_scan -> ScanCleaner) give identical frames for the same raw readings."""
    rng = np.random.default_rng(0)
    p = sample_params(rng)
    ad, cl = LidarAdapter(), ScanCleaner()
    for k in range(12):
        raw = lds02_corrupt(scene(RAY_ANGLES) - 0.05 * k, rng, p)
        ad.push(raw, 0.0, 6.28 / 359, 0.2 * k)
        sim, _ = cl(raw, 0.2 * k)
        assert np.array_equal(ad.frames[-1], sim)
        assert np.array_equal(ad.front_beams(0), sim[FRONT_IDX])
    x, v = ad.window()
    assert x.shape == (20, 360) and v.sum() == 12 and x.min() >= 0.12 and x.max() <= 3.5
    print('sim / robot pipelines identical')


def test_timing():
    ad = LidarAdapter()
    added = [ad.push(np.full(360, 2.0), 0.0, 6.28 / 359, t) for t in np.arange(0, 2.0, 0.1)]     # a 10 Hz lidar
    assert sum(added) == 10
    ad = LidarAdapter()
    added = [ad.push(np.full(360, 2.0), 0.0, 6.28 / 359, t) for t in np.arange(0, 2.0, 1 / 5.8)]  # a 5.8 Hz LDS-02
    assert all(added)
    print('frame timing ok (10 Hz -> every other scan, 5.8 Hz -> every scan)')


if __name__ == '__main__':
    test_layouts()
    test_invalid_and_hold()
    test_sim_equivalence()
    test_timing()
    print('ALL TESTS PASSED')