dash ble: implement retries when receiving messages

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/io/BleIO.kt

@@ -129,6 +129,6 @@ class BleIO(
 
     companion object {
 
-        private const val DEFAULT_IO_TIMEOUT_MS = 2000.toLong()
+        private const val DEFAULT_IO_TIMEOUT_MS = 1000.toLong()
     }
 }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/IncorrectPacketException.kt

@@ -3,6 +3,6 @@ package info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.message
 import info.nightscout.androidaps.utils.extensions.toHex
 
 class IncorrectPacketException(
-    val expectedIndex: Byte,
+    val payload: ByteArray,
-    val payload: ByteArray
+    val expectedIndex: Byte? = null
 ) : Exception("Invalid payload: ${payload.toHex()}. Expected index: $expectedIndex")

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/MessageIO.kt

@@ -10,9 +10,14 @@ import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.Chara
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.io.PayloadJoiner
 import info.nightscout.androidaps.plugins.pump.omnipod.dash.driver.comm.packet.BlePacket
 import info.nightscout.androidaps.utils.extensions.toHex
+import java.util.concurrent.TimeoutException
 
 class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
 
+    val receivedOutOfOrder = LinkedHashMap<Byte, ByteArray>()
+    var maxTries = 3
+    var tries = 0
+
     private fun expectCommandType(actual: BleCommand, expected: BleCommand) {
         if (actual.data.isEmpty()) {
             throw UnexpectedCommandException(actual)
@@ -80,33 +85,72 @@ class MessageIO(private val aapsLogger: AAPSLogger, private val bleIO: BleIO) {
         expectCommandType(BleCommand(expectSuccess), BleCommandSuccess())
     }
 
+    private fun expectBlePacket(index: Byte): ByteArray {
+        receivedOutOfOrder[index]?.let {
+            return it
+        }
+        while (tries < maxTries) {
+            try {
+                tries++
+                val payload = bleIO.receivePacket(CharacteristicType.DATA)
+                if (payload.isEmpty()) {
+                    throw IncorrectPacketException(payload, index)
+                }
+                if (payload[0] == index) {
+                    return payload
+                }
+                receivedOutOfOrder[payload[0]] = payload
+                bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(index).data)
+            } catch (e: TimeoutException) {
+                bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(index).data)
+                continue
+            }
+        }
+        throw MessageIOException("Ran out retries trying to receive a packet. $maxTries")
+    }
+
+    private fun readReset() {
+        maxTries = 3
+        tries = 0
+        receivedOutOfOrder.clear()
+    }
+
     fun receiveMessage(): MessagePacket {
         val expectRTS = bleIO.receivePacket(CharacteristicType.CMD, MESSAGE_READ_TIMEOUT_MS)
         expectCommandType(BleCommand(expectRTS), BleCommandRTS())
         bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandCTS().data)
+        readReset()
+        var expected: Byte = 0
         try {
-            val joiner = PayloadJoiner(bleIO.receivePacket(CharacteristicType.DATA))
+            val firstPacket = expectBlePacket(0)
+            val joiner = PayloadJoiner(firstPacket)
+            maxTries = joiner.fullFragments * 2 + 2
             for (i in 1 until joiner.fullFragments + 1) {
-                joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
+                expected++
+                val packet = expectBlePacket(expected)
+                joiner.accumulate(packet)
             }
             if (joiner.oneExtraPacket) {
-                joiner.accumulate(bleIO.receivePacket(CharacteristicType.DATA))
+                expected++
+                joiner.accumulate(expectBlePacket(expected))
             }
             val fullPayload = joiner.finalize()
             bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandSuccess().data)
             return MessagePacket.parse(fullPayload)
         } catch (e: IncorrectPacketException) {
-            aapsLogger.warn(LTag.PUMPBTCOMM, "Received incorrect packet: $e")
+            aapsLogger.warn(LTag.PUMPBTCOMM, "Could not read message: $e")
-            bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandNack(e.expectedIndex).data)
+            bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandAbort().data)
             throw MessageIOException(cause = e)
         } catch (e: CrcMismatchException) {
             aapsLogger.warn(LTag.PUMPBTCOMM, "CRC mismatch: $e")
             bleIO.sendAndConfirmPacket(CharacteristicType.CMD, BleCommandFail().data)
             throw MessageIOException(cause = e)
         }
+        receivedOutOfOrder.clear()
     }
 
     companion object {
-        private const val MESSAGE_READ_TIMEOUT_MS = 4000.toLong()
+
+        private const val MESSAGE_READ_TIMEOUT_MS = 2500.toLong()
     }
 }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/message/PayloadJoiner.kt

@@ -29,11 +29,11 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
 
     fun accumulate(packet: ByteArray) {
         if (packet.size < 3) { // idx, size, at least 1 byte of payload
-            throw IncorrectPacketException((expectedIndex + 1).toByte(), packet)
+            throw IncorrectPacketException(packet, (expectedIndex + 1).toByte())
         }
         val idx = packet[0].toInt()
         if (idx != expectedIndex + 1) {
-            throw IncorrectPacketException((expectedIndex + 1).toByte(), packet)
+            throw IncorrectPacketException(packet, (expectedIndex + 1).toByte())
         }
         expectedIndex++
         when {
@@ -53,7 +53,7 @@ class PayloadJoiner(private val firstPacket: ByteArray) {
             }
 
             idx > fullFragments -> {
-                throw IncorrectPacketException(idx.toByte(), packet)
+                throw IncorrectPacketException(packet, idx.toByte())
             }
         }
     }

omnipod-dash/src/main/java/info/nightscout/androidaps/plugins/pump/omnipod/dash/driver/comm/packet/BlePacket.kt

@@ -47,25 +47,25 @@ data class FirstBlePacket(
 
         fun parse(payload: ByteArray): FirstBlePacket {
             if (payload.size < FirstBlePacket.HEADER_SIZE_WITH_MIDDLE_PACKETS) {
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             if (payload[0].toInt() != 0) {
                 // most likely we lost the first packet.
                 // TODO: try to recover with NACKs?
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             val fullFragments = payload[1].toInt()
             require(fullFragments < MAX_FRAGMENTS) { "Received more than ${MAX_FRAGMENTS} fragments" }
             when {
                 // Without middle packets
                 payload.size < HEADER_SIZE_WITHOUT_MIDDLE_PACKETS ->
-                    throw IncorrectPacketException(0, payload)
+                    throw IncorrectPacketException(payload, 0)
 
                 fullFragments == 0 -> {
                     val rest = payload[6]
                     val end = Integer.min(rest + HEADER_SIZE_WITHOUT_MIDDLE_PACKETS, payload.size)
                     if (end > payload.size) {
-                        throw IncorrectPacketException(0, payload)
+                        throw IncorrectPacketException(payload, 0)
                     }
                     return FirstBlePacket(
                         fullFragments = fullFragments,
@@ -78,7 +78,7 @@ data class FirstBlePacket(
 
                 // With middle packets
                 payload.size < BlePacket.MAX_SIZE ->
-                    throw IncorrectPacketException(0, payload)
+                    throw IncorrectPacketException(payload, 0)
 
                 else -> {
                     return FirstBlePacket(
@@ -112,7 +112,7 @@ data class MiddleBlePacket(val index: Byte, override val payload: ByteArray) : B
 
         fun parse(payload: ByteArray): MiddleBlePacket {
             if (payload.size < MAX_SIZE) {
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             return MiddleBlePacket(
                 index = payload[0],
@@ -150,12 +150,12 @@ data class LastBlePacket(
 
         fun parse(payload: ByteArray): LastBlePacket {
             if (payload.size < HEADER_SIZE) {
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             val rest = payload[1]
             val end = Integer.min(rest + HEADER_SIZE, payload.size)
             if (payload.size < end) {
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             return LastBlePacket(
                 index = payload[0],
@@ -185,7 +185,7 @@ data class LastOptionalPlusOneBlePacket(
         fun parse(payload: ByteArray): LastOptionalPlusOneBlePacket {
             val size = payload[1].toInt()
             if (payload.size < HEADER_SIZE + size) {
-                throw IncorrectPacketException(0, payload)
+                throw IncorrectPacketException(payload, 0)
             }
             return LastOptionalPlusOneBlePacket(
                 index = payload[0],