// Protocol details ported from gan-web-bluetooth by Andy Fedotov, MIT License
// (https://github.com/afedotov/gan-web-bluetooth).
package coffee.liz.rookery.bt

import kotlin.math.roundToLong

/**
 * Reads arbitrary bit words from a message, MSB-first; 16/32-bit words may be little-endian.
 * Bits past the end read as 0.
 */
class GanBitReader(private val msg: ByteArray) {
    private fun bit(i: Int): Int = if (i ushr 3 >= msg.size) 0 else (msg[i ushr 3].toInt() ushr (7 - (i and 7))) and 1

    fun bits(start: Int, length: Int, littleEndian: Boolean = false): Int = when {
        length <= 8 -> {
            var v = 0
            for (i in start until start + length) v = (v shl 1) or bit(i)
            v
        }
        length == 16 || length == 32 -> {
            val n = length / 8
            val bytes = IntArray(n) { bits(start + 8 * it, 8) }
            var v = 0L
            for (i in 0 until n) v = (v shl 8) or (if (littleEndian) bytes[n - 1 - i] else bytes[i]).toLong()
            v.toInt()
        }
        else -> throw IllegalArgumentException("Unsupported bit word length $length")
    }

    /** Unsigned 32-bit word as Long. */
    fun u32(start: Int, littleEndian: Boolean = false): Long = bits(start, 32, littleEndian).toLong() and 0xFFFFFFFFL
}

/** Writes bit words MSB-first; the inverse of [GanBitReader] (used by tests and encoders). */
class GanBitWriter(size: Int) {
    val bytes = ByteArray(size)

    fun put(start: Int, length: Int, value: Long, littleEndian: Boolean = false): GanBitWriter {
        if (length == 16 || length == 32) {
            val n = length / 8
            for (i in 0 until n) {
                val shift = if (littleEndian) 8 * i else 8 * (n - 1 - i)
                put(start + 8 * i, 8, (value ushr shift) and 0xFF)
            }
            return this
        }
        require(length <= 8)
        for (i in 0 until length) {
            val b = ((value ushr (length - 1 - i)) and 1L).toInt()
            val pos = start + i
            val idx = pos ushr 3
            val mask = 1 shl (7 - (pos and 7))
            bytes[idx] = (if (b == 1) bytes[idx].toInt() or mask else bytes[idx].toInt() and mask.inv()).toByte()
        }
        return this
    }

    fun put(start: Int, length: Int, value: Int, littleEndian: Boolean = false) = put(start, length, value.toLong() and 0xFFFFFFFFL, littleEndian)
}

object GanFacelets {
    private val CORNER_FACELET_MAP = arrayOf(
        intArrayOf(8, 9, 20), intArrayOf(6, 18, 38), intArrayOf(0, 36, 47), intArrayOf(2, 45, 11),
        intArrayOf(29, 26, 15), intArrayOf(27, 44, 24), intArrayOf(33, 53, 42), intArrayOf(35, 17, 51),
    )
    private val EDGE_FACELET_MAP = arrayOf(
        intArrayOf(5, 10), intArrayOf(7, 19), intArrayOf(3, 37), intArrayOf(1, 46),
        intArrayOf(32, 16), intArrayOf(28, 25), intArrayOf(30, 43), intArrayOf(34, 52),
        intArrayOf(23, 12), intArrayOf(21, 41), intArrayOf(50, 39), intArrayOf(48, 14),
    )
    private const val FACES = "URFDLB"

    const val SOLVED = "UUUUUUUUURRRRRRRRRFFFFFFFFFDDDDDDDDDLLLLLLLLLBBBBBBBBB"

    /** Corner/edge permutation+orientation to a Kociemba facelets string. */
    fun toKociemba(s: CubeState): String {
        val f = CharArray(54) { FACES[it / 9] }
        for (i in 0 until 8) for (p in 0 until 3)
            f[CORNER_FACELET_MAP[i][(p + s.co[i]) % 3]] = FACES[CORNER_FACELET_MAP[s.cp[i]][p] / 9]
        for (i in 0 until 12) for (p in 0 until 2)
            f[EDGE_FACELET_MAP[i][(p + s.eo[i]) % 2]] = FACES[EDGE_FACELET_MAP[s.ep[i]][p] / 9]
        return f.concatToString()
    }

    /** Fill in the 8th corner / 12th edge from the first 7/11 (the cube only sends those). */
    fun complete(cp7: List<Int>, co7: List<Int>, ep11: List<Int>, eo11: List<Int>): CubeState = CubeState(
        cp = cp7 + (28 - cp7.sum()),
        co = co7 + ((3 - co7.sum() % 3) % 3),
        ep = ep11 + (66 - ep11.sum()),
        eo = eo11 + ((2 - eo11.sum() % 2) % 2),
    )
}

/** A move with optional timestamps, for [GanTimestampFit]. */
data class TimedMove(val move: String, val hostTimestampMs: Long?, val cubeTimestampMs: Long?)

object GanTimestampFit {
    private fun linregress(x: List<Long?>, y: List<Long?>): Pair<Double, Double> {
        var sumX = 0.0; var sumY = 0.0; var sumXY = 0.0; var sumXX = 0.0; var n = 0
        for (i in x.indices) {
            val xi = x[i]?.toDouble() ?: continue
            val yi = y[i]?.toDouble() ?: continue
            n++; sumX += xi; sumY += yi; sumXY += xi * yi; sumXX += xi * xi
        }
        val varX = n * sumXX - sumX * sumX
        val covXY = n * sumXY - sumX * sumY
        val slope = if (varX < 1e-3) 1.0 else covXY / varX
        val intercept = if (n < 1) 0.0 else sumY / n - slope * sumX / n
        return slope to intercept
    }

    /**
     * Fit cube-clock timestamps to host timestamps by linear regression, first filling missing cube
     * timestamps from neighbours (next -50 ms, then previous +50 ms). Returned cube timestamps are in
     * host-clock milliseconds relative to the first move (so the first is 0).
     */
    fun linearFit(moves: List<TimedMove>): List<TimedMove> {
        if (moves.isEmpty()) return emptyList()
        val cube = moves.map { it.cubeTimestampMs }.toMutableList()
        if (cube.size >= 2) {
            for (i in cube.size - 1 downTo 1) if (cube[i] != null && cube[i - 1] == null) cube[i - 1] = cube[i]!! - 50
            for (i in 0 until cube.size - 1) if (cube[i] != null && cube[i + 1] == null) cube[i + 1] = cube[i]!! + 50
        }
        val (slope, intercept) = linregress(cube, moves.map { it.hostTimestampMs })
        val first = cube[0]?.let { (slope * it + intercept).roundToLong() }
        return moves.mapIndexed { i, m ->
            val c = cube[i]
            m.copy(cubeTimestampMs = if (c == null || first == null) null else (slope * c + intercept).roundToLong() - first)
        }
    }

    /** Skew of the cube clock vs the host clock, in percent. */
    fun skewPercent(moves: List<TimedMove>): Double {
        if (moves.isEmpty()) return 0.0
        val (slope, _) = linregress(moves.map { it.hostTimestampMs }, moves.map { it.cubeTimestampMs })
        return ((slope - 1) * 100000).roundToLong() / 1000.0
    }
}