package org.terst.nav.nmea

import org.terst.nav.ais.AisVessel

class AisVdmParser {
    // Keyed by seqId -> list of (seq, payload)
    private val fragments = mutableMapOf<String, MutableList<Pair<Int, String>>>()

    fun parse(sentence: String): AisVessel? {
        if (!sentence.startsWith("!AIVDM") && !sentence.startsWith("!AIVDO")) return null

        val withoutBang = sentence.drop(1)
        val starIdx = withoutBang.indexOf('*')
        val body = if (starIdx >= 0) withoutBang.substring(0, starIdx) else withoutBang

        val fields = body.split(",")
        if (fields.size < 7) return null

        val count = fields[1].toIntOrNull() ?: return null
        val seq = fields[2].toIntOrNull() ?: return null
        val seqId = fields[3]
        val payload = fields[5]
        val padding = fields[6].toIntOrNull() ?: 0

        val combinedPayload: String
        if (count <= 1) {
            combinedPayload = payload
        } else {
            val list = fragments.getOrPut(seqId) { mutableListOf() }
            list.add(seq to payload)
            if (list.size < count) return null
            fragments.remove(seqId)
            combinedPayload = list.sortedBy { it.first }.joinToString("") { it.second }
        }

        val bits = decodeToBits(combinedPayload, padding)
        if (bits.size < 6) return null

        return when (extractUInt(bits, 0, 6)) {
            1, 2, 3 -> parseType123(bits)
            5 -> parseType5(bits)
            else -> null
        }
    }

    private fun decodeToBits(payload: String, padding: Int): IntArray {
        val allBits = ArrayList<Int>(payload.length * 6)
        for (c in payload) {
            var v = c.code - 48
            if (v > 39) v -= 8
            for (b in 5 downTo 0) allBits.add((v ushr b) and 1)
        }
        return if (padding > 0 && padding < allBits.size)
            allBits.dropLast(padding).toIntArray()
        else
            allBits.toIntArray()
    }

    private fun extractUInt(bits: IntArray, start: Int, len: Int): Int {
        var v = 0
        for (i in 0 until len) {
            val bit = if (start + i < bits.size) bits[start + i] else 0
            v = (v shl 1) or bit
        }
        return v
    }

    private fun extractInt(bits: IntArray, start: Int, len: Int): Int {
        val unsigned = extractUInt(bits, start, len)
        return if (len > 0 && (unsigned and (1 shl (len - 1))) != 0)
            unsigned - (1 shl len)
        else
            unsigned
    }

    private fun parseType123(bits: IntArray): AisVessel? {
        if (bits.size < 137) return null
        val mmsi = extractUInt(bits, 8, 30)
        val sogRaw = extractUInt(bits, 50, 10)
        val sog = if (sogRaw == 1023) 0.0 else sogRaw / 10.0
        val lonRaw = extractInt(bits, 61, 28)
        val lon = if (lonRaw == 0x6791AC0) 0.0 else lonRaw / 600000.0
        val latRaw = extractInt(bits, 89, 27)
        val lat = if (latRaw == 0x3412140) 0.0 else latRaw / 600000.0
        val cogRaw = extractUInt(bits, 116, 12)
        val cog = if (cogRaw == 3600) 0.0 else cogRaw / 10.0
        val heading = extractUInt(bits, 128, 9)
        return AisVessel(
            mmsi = mmsi,
            name = "",
            callsign = "",
            lat = lat,
            lon = lon,
            sog = sog,
            cog = cog,
            heading = heading,
            vesselType = 0,
            timestampMs = System.currentTimeMillis()
        )
    }

    private fun parseType5(bits: IntArray): AisVessel? {
        if (bits.size < 240) return null
        val mmsi = extractUInt(bits, 8, 30)
        val callsign = decodeText(bits, 70, 7).trimEnd('@', ' ')
        val name = decodeText(bits, 112, 20).trimEnd('@', ' ')
        val vesselType = extractUInt(bits, 232, 8)
        return AisVessel(
            mmsi = mmsi,
            name = name,
            callsign = callsign,
            lat = 0.0, lon = 0.0, sog = 0.0, cog = 0.0,
            heading = 511,
            vesselType = vesselType,
            timestampMs = System.currentTimeMillis()
        )
    }

    // AIS 6-bit text: bits<32 -> bits+64 (maps to @,A-Z,...), else bits (maps to space,digits,symbols)
    private fun decodeText(bits: IntArray, start: Int, nChars: Int): String {
        val sb = StringBuilder(nChars)
        for (i in 0 until nChars) {
            val v = extractUInt(bits, start + i * 6, 6)
            val c = if (v < 32) v + 64 else v
            sb.append(c.toChar())
        }
        return sb.toString()
    }
}