MicrOBU/app/src/test/java/com/hawhamburg/micr0bu/CamTxPvSerialTest.kt

package com.hawhamburg.micr0bu

import com.hawhamburg.micr0bu.data.transport.EspLinkStatus
import com.hawhamburg.micr0bu.data.transport.GnPositionVector
import com.hawhamburg.micr0bu.domain.asn1.ItsTime
import com.hawhamburg.micr0bu.domain.cam.Cam
import com.hawhamburg.micr0bu.domain.cam.StationType
import org.junit.Assert.assertEquals
import org.junit.Assert.assertFalse
import org.junit.Assert.assertTrue
import org.junit.Test

/**
 * Pins the phone side of SERIAL_MSG_CAM_TX_PV: the 24-byte prefix the ESP32 turns into the
 * GeoNetworking Source Position Vector, and the heartbeat capability bit that decides whether the
 * phone may send that message at all.
 *
 * ## Where the expected bytes come from
 * Not from this code. They were produced with Python's `struct.pack("<IiihH", ...)` from the
 * layout documented at SERIAL_MSG_CAM_TX_PV in `serial_link.h`, independently of this encoder, so
 * an agreement here is not an encoder agreeing with itself.
 *
 * That same `struct.pack` call is what the bench harness used on 2026-09-10 to drive an
 * ESP32-C5 over its native USB port with this message. The CiT One OBU, an independent
 * GeoNetworking stack, decoded every Source Position Vector field of the resulting
 * transmissions (station type, PAI, latitude, longitude, speed, heading and timestamp) back to
 * the values sent. These are bytes a third-party receiver has accepted on air, not only bytes
 * this app agrees with.
 */
class CamTxPvSerialTest {

    private fun String.hexToBytes(): ByteArray =
        chunked(2).map { it.toInt(16).toByte() }.toByteArray()

    private fun ByteArray.u32le(at: Int): Long =
        (0 until 4).fold(0L) { acc, i -> acc or ((this[at + i].toLong() and 0xFF) shl (8 * i)) }

    // ---- the wire layout -------------------------------------------------------------------

    @Test
    fun `encodes the prefix byte for byte`() {
        val pv = GnPositionVector(
            mac = "024d49435230".hexToBytes(),
            stationType = 2,
            pai = true,
            tstMs = 0x12345678L,
            latTenMicroDeg = 535_543_026,
            lonTenMicroDeg = 100_226_476,
            speedCms = 543,
            headingDeciDeg = 1234,
        )
        // 024d49435230 | 02 | 01 | 78563412 | f2bceb1f | ac55f905 | 1f02 | d204
        assertEquals("024d49435230020178563412f2bceb1fac55f9051f02d204", pv.toSerialPrefix().toHex())
    }

    @Test
    fun `encodes negative, extreme and flag-clear values`() {
        // Southern and western hemisphere, full reverse speed, heading at its maximum, PAI clear:
        // the sign handling that a northern-hemisphere bench test never exercises.
        val pv = GnPositionVector(
            mac = "020000000001".hexToBytes(),
            stationType = 2,
            pai = false,
            tstMs = 0xFFFF_FFFFL,
            latTenMicroDeg = -335_543_026,
            lonTenMicroDeg = -100_226_476,
            speedCms = -16384,
            headingDeciDeg = 3599,
        )
        assertEquals("0200000000010200ffffffff0e0500ec54aa06fa00c00f0e", pv.toSerialPrefix().toHex())
    }

    @Test
    fun `the timestamp is reduced modulo 2^32 on the wire`() {
        // TimestampIts passed 2^32 ms about 49.7 days after its 2004 epoch, so every real value
        // today is wider than 32 bits and the reduction is the normal case, not an edge case.
        val pv = vectorAt(tstMs = 716_121_572_779L)
        assertEquals(3_157_001_643L, pv.toSerialPrefix().u32le(8))
    }

    // ---- building it from a CAM ------------------------------------------------------------

    private val cam = Cam(
        stationId = 1_234_567_890L,
        stationType = StationType.CYCLIST,
        latitude = 53.5543026,
        longitude = 10.0226476,
        speedMps = 5.43,
        headingDeg = 123.4,
        yawRateDps = null,
        accelerationMps2 = null,
        timestamp = 1_789_036_772_779L,
        isOwn = true,
    )

    @Test
    fun `fromCam takes the same values the CAM payload carries`() {
        val pv = GnPositionVector.fromCam(cam, accuracyM = 5f, mac = "024d49435230".hexToBytes())
        assertEquals(2, pv.stationType)
        assertEquals(535_543_026, pv.latTenMicroDeg)
        assertEquals(100_226_476, pv.lonTenMicroDeg)
        assertEquals(543, pv.speedCms)
        assertEquals(1234, pv.headingDeciDeg)
        assertTrue(pv.pai)
        // The GN TST and the CAM's generationDeltaTime must follow one time rule.
        assertEquals(ItsTime.timestampIts(cam.timestamp), pv.tstMs)
        assertEquals(716_121_572_779L, pv.tstMs)
    }

    @Test
    fun `speed is clamped to the 15-bit field, never wrapped`() {
        // A wrapped 15-bit speed flips its sign bit and reads as reversing at speed.
        assertEquals(16383, GnPositionVector.fromCam(cam.copy(speedMps = 400.0), 5f, mac).speedCms)
        assertEquals(-16384, GnPositionVector.fromCam(cam.copy(speedMps = -400.0), 5f, mac).speedCms)
    }

    @Test
    fun `heading wraps into 0 to 3599`() {
        assertEquals(0, GnPositionVector.fromCam(cam.copy(headingDeg = 360.0), 5f, mac).headingDeciDeg)
        assertEquals(50, GnPositionVector.fromCam(cam.copy(headingDeg = 725.0), 5f, mac).headingDeciDeg)
        assertEquals(3590, GnPositionVector.fromCam(cam.copy(headingDeg = -1.0), 5f, mac).headingDeciDeg)
    }

    @Test
    fun `non-finite speed or heading does not throw`() {
        val pv = GnPositionVector.fromCam(
            cam.copy(speedMps = Double.NaN, headingDeg = Double.POSITIVE_INFINITY), 5f, mac,
        )
        assertEquals(0, pv.speedCms)
        assertEquals(0, pv.headingDeciDeg)
    }

    @Test
    fun `PAI follows the horizontal accuracy`() {
        assertTrue(GnPositionVector.fromCam(cam, GnPositionVector.PAI_MAX_ACCURACY_M, mac).pai)
        assertFalse(GnPositionVector.fromCam(cam, 25f, mac).pai)
        // Android reports 0 when it has no accuracy estimate: unknown is not accurate.
        assertFalse(GnPositionVector.fromCam(cam, 0f, mac).pai)
        assertFalse(GnPositionVector.fromCam(cam, null, mac).pai)
    }

    @Test(expected = IllegalArgumentException::class)
    fun `an address that is not six bytes is rejected`() {
        GnPositionVector.fromCam(cam, 5f, ByteArray(5))
    }

    // ---- capability negotiation ------------------------------------------------------------

    @Test
    fun `firmware that predates the capability byte advertises nothing`() {
        // Old firmware sends a 7-byte heartbeat. Reading that as "no CAM_TX_PV" is what keeps a
        // new app on the legacy message, which that firmware still understands.
        val status = EspLinkStatus.parse("00000000000000".hexToBytes())!!
        assertEquals(0, status.capabilities)
        assertFalse(status.supportsCamTxPv)
    }

    @Test
    fun `firmware that advertises CAM_TX_PV is recognised`() {
        val status = EspLinkStatus.parse("0000000000000001".hexToBytes())!!
        assertTrue(status.supportsCamTxPv)
    }

    @Test
    fun `a capability byte without the CAM_TX_PV bit does not enable it`() {
        assertFalse(EspLinkStatus.parse("0000000000000002".hexToBytes())!!.supportsCamTxPv)
    }

    private val mac = "024d49435230".hexToBytes()

    private fun vectorAt(tstMs: Long) = GnPositionVector(
        mac = mac, stationType = 2, pai = false, tstMs = tstMs,
        latTenMicroDeg = 0, lonTenMicroDeg = 0, speedCms = 0, headingDeciDeg = 0,
    )

    private fun ByteArray.toHex() = joinToString("") { "%02x".format(it) }
}