package org.terst.nav.nmea

import org.junit.Assert.*
import org.junit.Test
import kotlin.math.abs

private fun encodeBits(bits: IntArray): String {
    val padded = if (bits.size % 6 == 0) bits else bits + IntArray(6 - bits.size % 6)
    val sb = StringBuilder()
    var i = 0
    while (i < padded.size) {
        var v = 0
        for (b in 0 until 6) v = (v shl 1) or padded[i + b]
        val c = if (v <= 39) v + 48 else v + 56
        sb.append(c.toChar())
        i += 6
    }
    return sb.toString()
}

private fun intToBits(value: Int, len: Int): IntArray {
    val arr = IntArray(len)
    for (i in 0 until len) arr[i] = (value ushr (len - 1 - i)) and 1
    return arr
}

private fun signedToBits(value: Int, len: Int): IntArray {
    val mask = if (len < 32) (1 shl len) - 1 else -1
    return intToBits(value and mask, len)
}

private fun buildType1Payload(
    mmsi: Int, sog10: Int, lon600000: Int, lat600000: Int, cog10: Int, heading: Int
): String {
    val bits = IntArray(168)
    fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) }
    set(0, intToBits(1, 6))
    set(6, intToBits(0, 2))
    set(8, intToBits(mmsi, 30))
    set(38, intToBits(0, 12))
    set(50, intToBits(sog10, 10))
    set(60, intToBits(0, 1))
    set(61, signedToBits(lon600000, 28))
    set(89, signedToBits(lat600000, 27))
    set(116, intToBits(cog10, 12))
    set(128, intToBits(heading, 9))
    return encodeBits(bits)
}

private fun buildType5Payload(mmsi: Int, callsign: String, name: String, vesselType: Int): String {
    val bits = IntArray(426)
    fun set(start: Int, arr: IntArray) { arr.copyInto(bits, start) }
    set(0, intToBits(5, 6))
    set(6, intToBits(0, 2))
    set(8, intToBits(mmsi, 30))
    set(38, intToBits(0, 2))
    set(40, intToBits(0, 30))
    val csFixed = callsign.padEnd(7, '@').take(7)
    for (i in 0 until 7) {
        val c = csFixed[i].code
        val v = if (c >= 64) c - 64 else c
        set(70 + i * 6, intToBits(v, 6))
    }
    val nameFixed = name.padEnd(20, '@').take(20)
    for (i in 0 until 20) {
        val c = nameFixed[i].code
        val v = if (c >= 64) c - 64 else c
        set(112 + i * 6, intToBits(v, 6))
    }
    set(232, intToBits(vesselType, 8))
    return encodeBits(bits)
}

private fun nmea0183Checksum(body: String): String {
    var xor = 0
    for (c in body) xor = xor xor c.code
    return xor.toString(16).uppercase().padStart(2, '0')
}

private fun makeVdm(payload: String, padding: Int = 0): String {
    val body = "AIVDM,1,1,,A,$payload,$padding"
    return "!$body*${nmea0183Checksum(body)}"
}

private fun makeVdmPart(count: Int, seq: Int, seqId: String, payload: String, padding: Int = 0): String {
    val body = "AIVDM,$count,$seq,$seqId,A,$payload,$padding"
    return "!$body*${nmea0183Checksum(body)}"
}

class AisVdmParserTest {

    private val parser = AisVdmParser()

    @Test
    fun `non-AIS NMEA sentence returns null`() {
        assertNull(parser.parse("\$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A"))
    }

    @Test
    fun `malformed sentence returns null`() {
        assertNull(parser.parse("garbage"))
        assertNull(parser.parse("!AIVDM,1,1,,A,"))
        assertNull(parser.parse(""))
    }

    @Test
    fun `type-1 round-trip - known MMSI lat lon sog cog heading`() {
        val mmsi = 123456789
        val sog10 = 50
        val lon600000 = (-122.0 * 600000.0).toInt()
        val lat600000 = (37.0 * 600000.0).toInt()
        val cog10 = 2700
        val heading = 270

        val payload = buildType1Payload(mmsi, sog10, lon600000, lat600000, cog10, heading)
        val vessel = parser.parse(makeVdm(payload))

        assertNotNull(vessel)
        assertEquals(mmsi, vessel!!.mmsi)
        assertEquals(5.0, vessel.sog, 0.01)
        assertEquals(-122.0, vessel.lon, 0.001)
        assertEquals(37.0, vessel.lat, 0.001)
        assertEquals(270.0, vessel.cog, 0.1)
        assertEquals(270, vessel.heading)
        assertEquals("", vessel.name)
        assertEquals("", vessel.callsign)
        assertEquals(0, vessel.vesselType)
    }

    @Test
    fun `type-1 real sentence - MMSI 227006760`() {
        val vessel = parser.parse("!AIVDM,1,1,,A,13HOI:0P0000vocH;`5HF>0<0000,0*54")
        assertNotNull(vessel)
        assertEquals(227006760, vessel!!.mmsi)
    }

    @Test
    fun `type-5 round-trip - name and callsign decoded and trimmed`() {
        val mmsi = 987654321
        val payload = buildType5Payload(mmsi, "W1ABC", "MY VESSEL NAME", vesselType = 36)
        val vessel = parser.parse(makeVdm(payload))

        assertNotNull(vessel)
        assertEquals(mmsi, vessel!!.mmsi)
        assertEquals("W1ABC", vessel.callsign)
        assertEquals("MY VESSEL NAME", vessel.name)
        assertEquals(36, vessel.vesselType)
        assertEquals(0.0, vessel.lat, 0.0)
        assertEquals(0.0, vessel.lon, 0.0)
        assertEquals(511, vessel.heading)
    }

    @Test
    fun `two-part message reassembly`() {
        val mmsi = 111222333
        val payload = buildType1Payload(mmsi, 100, 0, 0, 900, 511)
        val half = payload.length / 2
        val s1 = makeVdmPart(2, 1, "1", payload.substring(0, half), 0)
        val s2 = makeVdmPart(2, 2, "1", payload.substring(half), 0)

        assertNull(parser.parse(s1))
        val vessel = parser.parse(s2)
        assertNotNull(vessel)
        assertEquals(mmsi, vessel!!.mmsi)
    }

    @Test
    fun `SOG not available value 1023 decodes to 0_0`() {
        val vessel = parser.parse(makeVdm(buildType1Payload(111000111, 1023, 0, 0, 0, 511)))
        assertNotNull(vessel)
        assertEquals(0.0, vessel!!.sog, 0.0)
    }

    @Test
    fun `COG not available value 3600 decodes to 0_0`() {
        val vessel = parser.parse(makeVdm(buildType1Payload(111000222, 0, 0, 0, 3600, 511)))
        assertNotNull(vessel)
        assertEquals(0.0, vessel!!.cog, 0.0)
    }
}