"""
Unit tests for saltybot_cellular node logic.
No ROS2 runtime required — tests pure functions mirrored from the nodes.
Run with: pytest jetson/ros2_ws/src/saltybot_cellular/test/test_cellular.py
"""

import math
import pytest


# ── Pure function mirrors — GPS ───────────────────────────────────────────────

KNOTS_TO_MS   = 0.514444
GPS_ACCURACY  = 2.5


def _nmea_checksum_valid(sentence):
    if '*' not in sentence:
        return False
    data, cs_str = sentence[1:].rsplit('*', 1)
    try:
        expected = int(cs_str.strip()[:2], 16)
    except ValueError:
        return False
    computed = 0
    for ch in data:
        computed ^= ord(ch)
    return computed == expected


def _nmea_dm_to_degrees(dm_str, direction):
    if not dm_str:
        return None
    dot = dm_str.index('.')
    deg  = float(dm_str[:dot - 2])
    mins = float(dm_str[dot - 2:])
    dec  = deg + mins / 60.0
    if direction in ('S', 'W'):
        dec = -dec
    return dec


def _parse_gga(sentence):
    raw = sentence.strip()
    if raw.startswith('$'):
        raw = raw[1:]
    if '*' in raw:
        raw = raw[:raw.index('*')]
    parts = raw.split(',')
    if len(parts) < 10 or not parts[0].endswith('GGA'):
        return None
    try:
        fix_quality = int(parts[6]) if parts[6] else 0
    except ValueError:
        return None
    if fix_quality == 0:
        return None
    lat = _nmea_dm_to_degrees(parts[2], parts[3])
    lon = _nmea_dm_to_degrees(parts[4], parts[5])
    if lat is None or lon is None:
        return None
    try:
        num_sats  = int(parts[7])   if parts[7]  else 0
        hdop      = float(parts[8]) if parts[8]  else 99.0
        alt_msl   = float(parts[9]) if parts[9]  else 0.0
        geoid_sep = float(parts[11]) if len(parts) > 11 and parts[11] else 0.0
    except (ValueError, IndexError):
        return None
    return {
        'latitude':       lat,
        'longitude':      lon,
        'altitude':       alt_msl + geoid_sep,
        'fix_quality':    fix_quality,
        'num_satellites': num_sats,
        'hdop':           hdop,
        'utc_time':       parts[1],
    }


def _parse_rmc(sentence):
    raw = sentence.strip()
    if raw.startswith('$'):
        raw = raw[1:]
    if '*' in raw:
        raw = raw[:raw.index('*')]
    parts = raw.split(',')
    if len(parts) < 9 or not parts[0].endswith('RMC'):
        return None
    if parts[2] != 'A':
        return None
    try:
        speed_mps  = float(parts[7]) * KNOTS_TO_MS if parts[7] else 0.0
        course_deg = float(parts[8]) if parts[8] else 0.0
    except ValueError:
        return None
    return {'speed_mps': speed_mps, 'course_deg': course_deg, 'valid': True}


def _hdop_to_covariance(hdop, accuracy=GPS_ACCURACY):
    sh = hdop * accuracy
    sv = sh * 1.5
    return [sh**2, 0, 0, 0, sh**2, 0, 0, 0, sv**2]


def _course_to_enu(speed_mps, course_deg):
    r = math.radians(course_deg)
    return speed_mps * math.sin(r), speed_mps * math.cos(r)  # vE, vN


# ── Pure function mirrors — AT parsing ───────────────────────────────────────

_CREG_STATUS = {
    "0": "not_registered", "1": "registered_home",
    "2": "searching",      "3": "denied",
    "4": "unknown",        "5": "roaming",
}


def _parse_csq(response):
    for line in response.splitlines():
        line = line.strip()
        if not line.startswith("+CSQ:"):
            continue
        parts = line[5:].strip().split(",")
        if len(parts) < 2:
            continue
        try:
            rssi_raw = int(parts[0].strip())
            ber      = int(parts[1].strip())
        except ValueError:
            continue
        if rssi_raw == 99:
            return {"rssi_raw": 99, "rssi_dbm": None, "ber": ber, "valid": False}
        return {"rssi_raw": rssi_raw, "rssi_dbm": -113 + 2 * rssi_raw,
                "ber": ber, "valid": True}
    return {"rssi_raw": 99, "rssi_dbm": None, "ber": 99, "valid": False}


def _parse_creg(response):
    for line in response.splitlines():
        line = line.strip()
        if not line.startswith("+CREG:"):
            continue
        parts = line[6:].strip().split(",")
        stat_str = parts[-1].strip().split()[0]
        return {"stat_raw": stat_str,
                "status_str": _CREG_STATUS.get(stat_str, "unknown"),
                "registered": stat_str in ("1", "5")}
    return {"stat_raw": "?", "status_str": "unknown", "registered": False}


def _parse_cops(response):
    for line in response.splitlines():
        if line.strip().startswith("+COPS:"):
            parts = line.strip()[6:].strip().split(",")
            if len(parts) >= 3:
                return parts[2].strip().strip('"')
    return ""


def _rssi_to_label(rssi_dbm):
    if rssi_dbm is None:
        return "no_signal"
    if rssi_dbm >= -70:
        return "excellent"
    if rssi_dbm >= -85:
        return "good"
    if rssi_dbm >= -100:
        return "fair"
    return "poor"


# ── Pure function mirrors — MQTT serialisation ────────────────────────────────

def _imu_to_dict(ax, ay, az, gx, gy, gz, sec=0):
    class Acc:
        pass
    class Msg:
        pass
    m = Msg()
    m.linear_acceleration = Acc()
    m.angular_velocity    = Acc()
    m.header              = Acc()
    m.header.stamp        = Acc()
    m.linear_acceleration.x = ax
    m.linear_acceleration.y = ay
    m.linear_acceleration.z = az
    m.angular_velocity.x    = gx
    m.angular_velocity.y    = gy
    m.angular_velocity.z    = gz
    m.header.stamp.sec      = sec
    return {
        "ax": round(ax, 3), "ay": round(ay, 3), "az": round(az, 3),
        "gx": round(gx, 4), "gy": round(gy, 4), "gz": round(gz, 4),
        "t": sec,
    }


# ── NMEA checksum tests ───────────────────────────────────────────────────────

class TestNmeaChecksum:
    def test_valid_gpgga(self):
        # Valid GGA from standard NMEA example
        s = "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47"
        assert _nmea_checksum_valid(s) is True

    def test_valid_gnrmc(self):
        s = "$GNRMC,123519.00,A,4807.0380,N,01131.0000,E,0.100,0.00,010101,,,A*7F"
        assert _nmea_checksum_valid(s) is True

    def test_invalid_checksum(self):
        s = "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*FF"
        assert _nmea_checksum_valid(s) is False

    def test_no_checksum(self):
        assert _nmea_checksum_valid("$GPGGA,123519,4807.038") is False

    def test_empty_string(self):
        assert _nmea_checksum_valid("") is False


# ── NMEA DM conversion tests ──────────────────────────────────────────────────

class TestNmeaDm:
    def test_north_hemisphere(self):
        # 4807.038N = 48° 7.038' = 48.1173°
        assert _nmea_dm_to_degrees("4807.038", "N") == pytest.approx(48.1173, abs=1e-4)

    def test_south_hemisphere_negative(self):
        result = _nmea_dm_to_degrees("4807.038", "S")
        assert result == pytest.approx(-48.1173, abs=1e-4)

    def test_east_longitude(self):
        # 01131.000E = 11° 31.000' = 11.5167°
        assert _nmea_dm_to_degrees("01131.000", "E") == pytest.approx(11.5167, abs=1e-4)

    def test_west_longitude_negative(self):
        result = _nmea_dm_to_degrees("07937.600", "W")
        assert result < 0

    def test_zero(self):
        assert _nmea_dm_to_degrees("0000.000", "N") == pytest.approx(0.0)

    def test_empty_string_none(self):
        assert _nmea_dm_to_degrees("", "N") is None


# ── GGA parsing tests ─────────────────────────────────────────────────────────

_GGA_VALID = "$GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47"
_GGA_DGPS  = "$GPGGA,123519,4807.038,N,01131.000,E,2,08,0.9,545.4,M,46.9,M,,*44"
_GGA_NOFIX = "$GPGGA,123519,,,,,0,00,99.9,,M,,M,,*71"
_GGA_GN    = "$GNGGA,123519,4807.038,N,01131.000,E,1,12,0.7,545.4,M,46.9,M,,*4B"


class TestParseGga:
    def test_valid_parse(self):
        r = _parse_gga(_GGA_VALID)
        assert r is not None
        assert r["latitude"]   == pytest.approx(48.1173, abs=1e-4)
        assert r["longitude"]  == pytest.approx(11.5167, abs=1e-4)
        assert r["fix_quality"] == 1
        assert r["num_satellites"] == 8
        assert r["hdop"] == pytest.approx(0.9)

    def test_altitude_is_msl_plus_geoid(self):
        r = _parse_gga(_GGA_VALID)
        # 545.4 MSL + 46.9 geoid = 592.3m ellipsoidal
        assert r["altitude"] == pytest.approx(592.3, abs=0.01)

    def test_no_fix_returns_none(self):
        assert _parse_gga(_GGA_NOFIX) is None

    def test_dgps_fix_quality_2(self):
        r = _parse_gga(_GGA_DGPS)
        assert r is not None
        assert r["fix_quality"] == 2

    def test_gn_talker_id_accepted(self):
        r = _parse_gga(_GGA_GN)
        assert r is not None
        assert r["num_satellites"] == 12

    def test_non_gga_returns_none(self):
        assert _parse_gga("$GPGSV,3,1,12,...*XX") is None

    def test_short_sentence_returns_none(self):
        assert _parse_gga("$GPGGA,123519") is None

    def test_empty_returns_none(self):
        assert _parse_gga("") is None

    def test_utc_time_field(self):
        r = _parse_gga(_GGA_VALID)
        assert r["utc_time"] == "123519"


# ── RMC parsing tests ─────────────────────────────────────────────────────────

_RMC_VALID = "$GNRMC,123519.00,A,4807.038,N,01131.000,E,1.234,181.5,010101,,,A*XX"
_RMC_VOID  = "$GNRMC,123519.00,V,,,,,,,010101,,,N*XX"


class TestParseRmc:
    def test_valid_speed(self):
        r = _parse_rmc(_RMC_VALID)
        assert r is not None
        assert r["speed_mps"] == pytest.approx(1.234 * KNOTS_TO_MS, rel=1e-3)

    def test_valid_course(self):
        r = _parse_rmc(_RMC_VALID)
        assert r["course_deg"] == pytest.approx(181.5, abs=0.01)

    def test_void_returns_none(self):
        assert _parse_rmc(_RMC_VOID) is None

    def test_non_rmc_returns_none(self):
        assert _parse_rmc(_GGA_VALID) is None

    def test_zero_speed(self):
        rmc = "$GNRMC,000000.00,A,4807.038,N,01131.000,E,0.000,0.0,010101,,,A*XX"
        r = _parse_rmc(rmc)
        assert r is not None
        assert r["speed_mps"] == pytest.approx(0.0)


# ── HDOP covariance tests ─────────────────────────────────────────────────────

class TestHdopCovariance:
    def test_hdop1_gives_accuracy_squared(self):
        cov = _hdop_to_covariance(1.0)
        assert cov[0] == pytest.approx(GPS_ACCURACY ** 2)   # sigma_h^2
        assert cov[4] == pytest.approx(GPS_ACCURACY ** 2)
        assert cov[8] == pytest.approx((GPS_ACCURACY * 1.5) ** 2)

    def test_hdop2_doubles_sigma(self):
        cov1 = _hdop_to_covariance(1.0)
        cov2 = _hdop_to_covariance(2.0)
        # sigma doubles → variance quadruples
        assert cov2[0] == pytest.approx(4.0 * cov1[0])

    def test_off_diagonal_zero(self):
        cov = _hdop_to_covariance(1.0)
        assert cov[1] == pytest.approx(0.0)
        assert cov[2] == pytest.approx(0.0)
        assert cov[3] == pytest.approx(0.0)

    def test_vertical_worse_than_horizontal(self):
        cov = _hdop_to_covariance(1.0)
        assert cov[8] > cov[0]   # sigma_v > sigma_h


# ── Course → ENU velocity tests ───────────────────────────────────────────────

class TestCourseToEnu:
    def test_north_course_zero_easting(self):
        vE, vN = _course_to_enu(1.0, 0.0)    # 0° = due North
        assert vE == pytest.approx(0.0, abs=1e-9)
        assert vN == pytest.approx(1.0)

    def test_east_course_zero_northing(self):
        vE, vN = _course_to_enu(1.0, 90.0)   # 90° = due East
        assert vE == pytest.approx(1.0)
        assert vN == pytest.approx(0.0, abs=1e-9)

    def test_south_course_negative_northing(self):
        vE, vN = _course_to_enu(1.0, 180.0)  # 180° = South
        assert vN == pytest.approx(-1.0)

    def test_speed_preserved(self):
        speed = 3.5
        vE, vN = _course_to_enu(speed, 45.0)
        assert math.sqrt(vE**2 + vN**2) == pytest.approx(speed)

    def test_zero_speed(self):
        vE, vN = _course_to_enu(0.0, 90.0)
        assert vE == pytest.approx(0.0)
        assert vN == pytest.approx(0.0)


# ── AT+CSQ parsing tests ──────────────────────────────────────────────────────

class TestParseCsq:
    def test_nominal_rssi(self):
        r = _parse_csq("+CSQ: 18,0\r\nOK\r\n")
        assert r["valid"]    is True
        assert r["rssi_raw"] == 18
        assert r["rssi_dbm"] == -113 + 2 * 18   # = -77 dBm
        assert r["ber"]      == 0

    def test_unknown_rssi_99(self):
        r = _parse_csq("+CSQ: 99,99\r\nOK\r\n")
        assert r["valid"]    is False
        assert r["rssi_dbm"] is None

    def test_min_rssi_0(self):
        r = _parse_csq("+CSQ: 0,0\r\nOK\r\n")
        assert r["rssi_dbm"] == -113

    def test_max_rssi_31(self):
        r = _parse_csq("+CSQ: 31,0\r\nOK\r\n")
        assert r["rssi_dbm"] == -113 + 62   # = -51 dBm

    def test_empty_response(self):
        r = _parse_csq("ERROR\r\n")
        assert r["valid"] is False

    def test_csq_without_space(self):
        # Some modems omit the space after +CSQ:
        r = _parse_csq("+CSQ:20,0\r\nOK\r\n")
        assert r["valid"]    is True
        assert r["rssi_raw"] == 20


# ── AT+CREG? parsing tests ─────────────────────────────────────────────────────

class TestParseCreg:
    def test_registered_home(self):
        r = _parse_creg("+CREG: 0,1\r\nOK\r\n")
        assert r["registered"]  is True
        assert r["status_str"]  == "registered_home"

    def test_roaming(self):
        r = _parse_creg("+CREG: 0,5\r\nOK\r\n")
        assert r["registered"]  is True
        assert r["status_str"]  == "roaming"

    def test_not_registered(self):
        r = _parse_creg("+CREG: 0,0\r\nOK\r\n")
        assert r["registered"]  is False

    def test_searching(self):
        r = _parse_creg("+CREG: 0,2\r\nOK\r\n")
        assert r["registered"]  is False
        assert r["status_str"]  == "searching"

    def test_stat_only_format(self):
        r = _parse_creg("+CREG: 1\r\nOK\r\n")
        assert r["registered"]  is True

    def test_empty_response(self):
        r = _parse_creg("ERROR\r\n")
        assert r["registered"]  is False


# ── AT+COPS? parsing tests ─────────────────────────────────────────────────────

class TestParseCops:
    def test_operator_name(self):
        r = _parse_cops('+COPS: 0,0,"Fido",7\r\nOK\r\n')
        assert r == "Fido"

    def test_operator_with_spaces(self):
        r = _parse_cops('+COPS: 0,0,"Rogers Wireless",7\r\nOK\r\n')
        assert r == "Rogers Wireless"

    def test_no_operator(self):
        r = _parse_cops("+COPS: 0\r\nOK\r\n")
        assert r == ""

    def test_error_response(self):
        r = _parse_cops("ERROR\r\n")
        assert r == ""


# ── Signal quality label tests ────────────────────────────────────────────────

class TestRssiLabel:
    def test_excellent(self):
        assert _rssi_to_label(-65) == "excellent"

    def test_good(self):
        assert _rssi_to_label(-80) == "good"

    def test_fair(self):
        assert _rssi_to_label(-95) == "fair"

    def test_poor(self):
        assert _rssi_to_label(-105) == "poor"

    def test_none_is_no_signal(self):
        assert _rssi_to_label(None) == "no_signal"

    def test_boundary_good_excellent(self):
        assert _rssi_to_label(-70) == "excellent"   # -70 >= -70 → excellent

    def test_boundary_fair_good(self):
        assert _rssi_to_label(-85) == "good"         # -85 >= -85 → good


# ── MQTT serialisation tests ──────────────────────────────────────────────────

class TestMqttSerialisation:
    def test_imu_dict_keys(self):
        d = _imu_to_dict(9.81, 0.1, -0.2, 0.01, -0.02, 0.0)
        assert set(d.keys()) == {"ax", "ay", "az", "gx", "gy", "gz", "t"}

    def test_imu_precision(self):
        d = _imu_to_dict(9.81234, 0.0, 0.0, 0.0, 0.0, 0.0)
        assert d["ax"] == pytest.approx(9.812, abs=1e-3)

    def test_gps_fix_dict(self):
        # Simple inline navsatfix
        class S: pass
        m = S(); m.status = S(); m.header = S(); m.header.stamp = S()
        m.latitude = 48.1173; m.longitude = 11.5167; m.altitude = 592.3
        m.status.status = 0; m.header.stamp.sec = 1234
        d = {
            "lat": round(m.latitude, 7), "lon": round(m.longitude, 7),
            "alt": round(m.altitude, 2), "stat": int(m.status.status),
            "t":   m.header.stamp.sec,
        }
        assert d["lat"] == pytest.approx(48.1173, abs=1e-4)
        assert d["alt"] == pytest.approx(592.3)
