#!/usr/bin/env python3
"""Unit tests for saltybot_dronecan_gps.dronecan_parser.

No dronecan library, ROS2, or CAN hardware required.
Uses simple Python objects (namespaces) to simulate dronecan message objects.

Run with:  pytest test/ -v
"""

import math
import types
import unittest

from saltybot_dronecan_gps.dronecan_parser import (
    DTID_FIX2,
    DTID_RAW_IMU,
    DTID_MAG_FIELD,
    DTID_STATIC_PRESSURE,
    DTID_STATIC_TEMPERATURE,
    DTID_NODE_STATUS,
    DTID_RTCM_STREAM,
    FIX_NO_FIX,
    FIX_TIME_ONLY,
    FIX_2D,
    FIX_3D,
    RTCM_MAX_CHUNK,
    chunk_rtcm,
    parse_fix2,
    parse_mag,
    parse_node_status,
    parse_raw_imu,
    parse_static_pressure,
    parse_static_temperature,
    _getf,
    _list3,
    _list9,
)


# ── Helpers ───────────────────────────────────────────────────────────────────

def _ns(**kwargs) -> types.SimpleNamespace:
    """Build a simple namespace to mimic a dronecan message object."""
    return types.SimpleNamespace(**kwargs)


def _ns_vec(*values):
    """Return an object that is indexable like a dronecan vector field."""
    return list(values)


# ── DTID constants ────────────────────────────────────────────────────────────

class TestDtidConstants(unittest.TestCase):
    def test_known_values(self):
        self.assertEqual(DTID_FIX2,               1063)
        self.assertEqual(DTID_RAW_IMU,             1003)
        self.assertEqual(DTID_MAG_FIELD,           1001)
        self.assertEqual(DTID_STATIC_PRESSURE,     1028)
        self.assertEqual(DTID_STATIC_TEMPERATURE,  1029)
        self.assertEqual(DTID_NODE_STATUS,          341)
        self.assertEqual(DTID_RTCM_STREAM,         1062)

    def test_rtcm_chunk_size(self):
        self.assertEqual(RTCM_MAX_CHUNK, 128)


# ── _getf / _list3 / _list9 ──────────────────────────────────────────────────

class TestHelpers(unittest.TestCase):
    def test_getf_simple(self):
        obj = _ns(x=42)
        self.assertEqual(_getf(obj, "x"), 42)

    def test_getf_missing(self):
        obj = _ns(x=1)
        self.assertIsNone(_getf(obj, "y"))

    def test_getf_default(self):
        obj = _ns()
        self.assertEqual(_getf(obj, "missing", default=99), 99)

    def test_list3_ok(self):
        self.assertEqual(_list3([1.0, 2.0, 3.0]), [1.0, 2.0, 3.0])

    def test_list3_bad(self):
        self.assertEqual(_list3(None), [0.0, 0.0, 0.0])

    def test_list9_ok(self):
        vals = list(range(9))
        self.assertEqual(_list9(vals), [float(v) for v in vals])

    def test_list9_short(self):
        self.assertEqual(_list9(None), [0.0] * 9)


# ── parse_fix2 ────────────────────────────────────────────────────────────────

class TestParseFix2(unittest.TestCase):

    def _make_fix(self, lat_1e8=0, lon_1e8=0, alt_msl_mm=0, alt_ell_mm=0,
                  ned_vel=None, status=FIX_3D, sats=12,
                  pdop=1.2, hdop=0.9, vdop=1.5,
                  pos_cov=None, undulation=5.0, gnss_ts=0):
        return _ns(
            latitude_deg_1e8  = lat_1e8,
            longitude_deg_1e8 = lon_1e8,
            height_msl_mm     = alt_msl_mm,
            height_ellipsoid_mm = alt_ell_mm,
            ned_velocity      = ned_vel if ned_vel is not None else _ns_vec(0.0, 0.0, 0.0),
            status            = status,
            sats_used         = sats,
            pdop              = pdop,
            hdop              = hdop,
            vdop              = vdop,
            position_covariance = pos_cov,
            undulation        = undulation,
            gnss_timestamp    = gnss_ts,
        )

    def test_basic_3d_fix(self):
        msg = self._make_fix(
            lat_1e8    = 4_576_543_210,  # 45.7654321°
            lon_1e8    = -7_345_678_901, # -73.45678901°
            alt_msl_mm = 50_000,         # 50 m
            alt_ell_mm = 52_000,         # 52 m
            ned_vel    = _ns_vec(1.5, -0.3, 0.0),
            sats       = 14,
            hdop       = 0.8,
        )
        d = parse_fix2(msg)
        self.assertIsNotNone(d)
        self.assertAlmostEqual(d["lat_deg"],       45.7654321, places=5)
        self.assertAlmostEqual(d["lon_deg"],       -73.45678901, places=5)
        self.assertAlmostEqual(d["alt_msl_m"],     50.0, places=3)
        self.assertAlmostEqual(d["alt_ellipsoid_m"], 52.0, places=3)
        self.assertEqual(d["ned_vel"], [1.5, -0.3, 0.0])
        self.assertEqual(d["fix_type"],  FIX_3D)
        self.assertEqual(d["fix_label"], "3D_FIX")
        self.assertEqual(d["sats_used"], 14)
        self.assertAlmostEqual(d["hdop"], 0.8, places=5)

    def test_no_fix(self):
        msg = self._make_fix(status=FIX_NO_FIX, sats=0)
        d = parse_fix2(msg)
        self.assertIsNotNone(d)
        self.assertEqual(d["fix_type"],  FIX_NO_FIX)
        self.assertEqual(d["fix_label"], "NO_FIX")
        self.assertEqual(d["sats_used"], 0)

    def test_time_only(self):
        msg = self._make_fix(status=FIX_TIME_ONLY)
        d = parse_fix2(msg)
        self.assertEqual(d["fix_label"], "TIME_ONLY")

    def test_2d_fix(self):
        msg = self._make_fix(status=FIX_2D)
        d = parse_fix2(msg)
        self.assertEqual(d["fix_label"], "2D_FIX")

    def test_position_covariance(self):
        cov = [float(i) for i in range(9)]
        msg = self._make_fix(pos_cov=cov)
        d = parse_fix2(msg)
        self.assertEqual(d["pos_cov"], cov)

    def test_no_covariance_returns_zeros(self):
        msg = self._make_fix(pos_cov=None)
        d = parse_fix2(msg)
        self.assertEqual(d["pos_cov"], [0.0] * 9)

    def test_zero_lat_lon(self):
        msg = self._make_fix(lat_1e8=0, lon_1e8=0)
        d = parse_fix2(msg)
        self.assertAlmostEqual(d["lat_deg"], 0.0, places=8)
        self.assertAlmostEqual(d["lon_deg"], 0.0, places=8)

    def test_returns_none_on_bad_input(self):
        self.assertIsNone(parse_fix2(None))

    def test_gnss_timestamp(self):
        msg = self._make_fix(gnss_ts=1_000_000_000)
        d = parse_fix2(msg)
        self.assertEqual(d["gnss_timestamp_us"], 1_000_000_000)


# ── parse_raw_imu ─────────────────────────────────────────────────────────────

class TestParseRawImu(unittest.TestCase):

    def _make_imu(self, gyro=None, accel=None, dt=0.02,
                  gyro_cov=None, accel_cov=None):
        return _ns(
            rate_gyro_latest         = gyro  if gyro  else _ns_vec(0.0, 0.0, 0.0),
            accelerometer_latest     = accel if accel else _ns_vec(0.0, 0.0, 9.81),
            integration_interval     = dt,
            rate_gyro_covariance     = gyro_cov  if gyro_cov  else [0.0] * 9,
            accelerometer_covariance = accel_cov if accel_cov else [0.0] * 9,
        )

    def test_basic(self):
        msg = self._make_imu(
            gyro  = _ns_vec(0.01, -0.02, 0.03),
            accel = _ns_vec(0.1, -0.2, 9.8),
            dt    = 0.02,
        )
        d = parse_raw_imu(msg)
        self.assertIsNotNone(d)
        self.assertEqual(d["gyro_xyz"],  [0.01, -0.02, 0.03])
        self.assertEqual(d["accel_xyz"], [0.1, -0.2, 9.8])
        self.assertAlmostEqual(d["dt_s"], 0.02)

    def test_covariances_returned(self):
        cov = [float(i * 0.001) for i in range(9)]
        msg = self._make_imu(gyro_cov=cov, accel_cov=cov)
        d = parse_raw_imu(msg)
        self.assertEqual(d["gyro_cov"],  cov)
        self.assertEqual(d["accel_cov"], cov)

    def test_none_input(self):
        self.assertIsNone(parse_raw_imu(None))


# ── parse_mag ─────────────────────────────────────────────────────────────────

class TestParseMag(unittest.TestCase):

    def _make_mag(self, sensor_id=0, field=None, cov=None):
        return _ns(
            sensor_id              = sensor_id,
            magnetic_field_ga      = field if field else _ns_vec(0.1, 0.2, -0.5),
            magnetic_field_covariance = cov if cov else [0.0] * 9,
        )

    def test_basic(self):
        msg = self._make_mag(sensor_id=1, field=_ns_vec(0.1, 0.2, -0.5))
        d = parse_mag(msg)
        self.assertIsNotNone(d)
        self.assertEqual(d["sensor_id"],  1)
        self.assertEqual(d["field_ga"],   [0.1, 0.2, -0.5])

    def test_none_input(self):
        self.assertIsNone(parse_mag(None))


# ── parse_static_pressure ────────────────────────────────────────────────────

class TestParseStaticPressure(unittest.TestCase):

    def test_basic(self):
        msg = _ns(static_pressure=101325.0, static_pressure_variance=100.0)
        d = parse_static_pressure(msg)
        self.assertIsNotNone(d)
        self.assertAlmostEqual(d["pressure_pa"],       101325.0)
        self.assertAlmostEqual(d["pressure_variance"], 100.0)

    def test_none_input(self):
        self.assertIsNone(parse_static_pressure(None))


# ── parse_static_temperature ─────────────────────────────────────────────────

class TestParseStaticTemperature(unittest.TestCase):

    def test_celsius_to_kelvin(self):
        msg = _ns(static_temperature=293.15)  # 20 °C
        d = parse_static_temperature(msg)
        self.assertIsNotNone(d)
        self.assertAlmostEqual(d["temperature_k"], 293.15)

    def test_none_input(self):
        self.assertIsNone(parse_static_temperature(None))


# ── parse_node_status ────────────────────────────────────────────────────────

class TestParseNodeStatus(unittest.TestCase):

    def _make_status(self, health=0, mode=0, uptime=120, vendor_code=0):
        return _ns(
            health                     = health,
            mode                       = mode,
            uptime_sec                 = uptime,
            vendor_specific_status_code = vendor_code,
        )

    def test_ok_operational(self):
        msg = self._make_status(health=0, mode=0, uptime=300)
        d = parse_node_status(msg, source_node_id=10)
        self.assertIsNotNone(d)
        self.assertEqual(d["node_id"],     10)
        self.assertEqual(d["health"],      0)
        self.assertEqual(d["health_label"], "OK")
        self.assertEqual(d["mode_label"],  "OPERATIONAL")
        self.assertEqual(d["uptime_sec"],  300)

    def test_error_state(self):
        msg = self._make_status(health=2, mode=0)
        d = parse_node_status(msg, source_node_id=5)
        self.assertEqual(d["health_label"], "ERROR")

    def test_critical_state(self):
        msg = self._make_status(health=3)
        d = parse_node_status(msg, 7)
        self.assertEqual(d["health_label"], "CRITICAL")

    def test_none_input(self):
        self.assertIsNone(parse_node_status(None, 1))


# ── chunk_rtcm ────────────────────────────────────────────────────────────────

class TestChunkRtcm(unittest.TestCase):

    def test_empty(self):
        chunks = chunk_rtcm(b"", 0)
        self.assertEqual(chunks, [])

    def test_single_chunk(self):
        data = bytes(range(64))
        chunks = chunk_rtcm(data, 5)
        self.assertEqual(len(chunks), 1)
        self.assertEqual(chunks[0]["sequence_id"], 5)
        self.assertEqual(chunks[0]["data"],        data)

    def test_exact_chunk_boundary(self):
        data = bytes(RTCM_MAX_CHUNK)
        chunks = chunk_rtcm(data, 0)
        self.assertEqual(len(chunks), 1)
        self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)

    def test_two_chunks(self):
        data = bytes(RTCM_MAX_CHUNK + 1)
        chunks = chunk_rtcm(data, 0)
        self.assertEqual(len(chunks), 2)
        self.assertEqual(len(chunks[0]["data"]), RTCM_MAX_CHUNK)
        self.assertEqual(len(chunks[1]["data"]), 1)

    def test_sequence_id_increments(self):
        data = bytes(RTCM_MAX_CHUNK * 3)
        chunks = chunk_rtcm(data, 10)
        for i, chunk in enumerate(chunks):
            self.assertEqual(chunk["sequence_id"], 10 + i)

    def test_sequence_id_wraps_at_16bit(self):
        data = bytes(RTCM_MAX_CHUNK * 2)
        chunks = chunk_rtcm(data, 0xFFFF)
        self.assertEqual(chunks[0]["sequence_id"], 0xFFFF)
        self.assertEqual(chunks[1]["sequence_id"], 0x0000)   # wraps

    def test_data_round_trip(self):
        original = bytes(range(200))
        chunks = chunk_rtcm(original, 0)
        reassembled = b"".join(c["data"] for c in chunks)
        self.assertEqual(reassembled, original)

    def test_large_message(self):
        data = bytes(1024)
        chunks = chunk_rtcm(data, 0)
        total_bytes = sum(len(c["data"]) for c in chunks)
        self.assertEqual(total_bytes, 1024)
        for c in chunks:
            self.assertLessEqual(len(c["data"]), RTCM_MAX_CHUNK)


if __name__ == "__main__":
    unittest.main()
