saltylab-ios/SulTee/SulTee/BLEPackets.swift

import Foundation
import CoreLocation
import CoreMotion

/// Builds BLE write packets in the exact binary format expected by the SaltyTag UWB firmware,
/// and parses incoming ranging notifications.
///
/// All multi-byte fields are little-endian.
enum BLEPackets {

    // MARK: - GPS packet (20 bytes)
    //
    // [0-3]   Int32  LE   latitude  × 1e7
    // [4-7]   Int32  LE   longitude × 1e7
    // [8-9]   Int16  LE   altitude  × 10  (dm, clamped ±32767)
    // [10-11] Uint16 LE   speed     × 100 (cm/s, clamped 0–65535)
    // [12-13] Uint16 LE   heading   × 100 (0.01°, clamped 0–35999)
    // [14]    Uint8       accuracy  × 10  (clamped 0–255)
    // [15]    Uint8       fix_type  (0=mocked 1=2D 2=3D)
    // [16-19] Uint32 LE   timestamp lower 32 bits of ms since epoch

    static func gpsPacket(from location: CLLocation) -> Data {
        var buf = Data(count: 20)

        let lat      = Int32(clamping: Int64((location.coordinate.latitude  * 1e7).rounded()))
        let lon      = Int32(clamping: Int64((location.coordinate.longitude * 1e7).rounded()))
        let altDm    = Int16(clamping: Int64((location.altitude * 10).rounded()))
        let speedCms = UInt16(clamping: Int64(max(0, location.speed * 100).rounded()))
        let course   = location.course >= 0 ? location.course : 0
        let hdg      = UInt16(clamping: Int64((course * 100).rounded()) % 36000)
        let acc      = UInt8(clamping: Int64(max(0, location.horizontalAccuracy * 10).rounded()))
        let fixType: UInt8 = location.horizontalAccuracy > 0 ? 2 : 1
        let tsMsLow  = UInt32(UInt64(location.timestamp.timeIntervalSince1970 * 1000) & 0xFFFFFFFF)

        buf.writeInt32LE(lat,      at: 0)
        buf.writeInt32LE(lon,      at: 4)
        buf.writeInt16LE(altDm,    at: 8)
        buf.writeUInt16LE(speedCms, at: 10)
        buf.writeUInt16LE(hdg,     at: 12)
        buf[14] = acc
        buf[15] = fixType
        buf.writeUInt32LE(tsMsLow, at: 16)

        return buf
    }

    // MARK: - IMU packet (22 bytes)
    //
    // [0-1]   Int16 LE  accel X  milli-g  (m/s² already in g in CoreMotion → ×1000)
    // [2-3]   Int16 LE  accel Y
    // [4-5]   Int16 LE  accel Z
    // [6-7]   Int16 LE  gyro  X  centi-deg/s  (rad/s × 5729.578)
    // [8-9]   Int16 LE  gyro  Y
    // [10-11] Int16 LE  gyro  Z
    // [12-13] Int16 LE  mag   X  μT
    // [14-15] Int16 LE  mag   Y
    // [16-17] Int16 LE  mag   Z
    // [18-21] Uint32 LE timestamp lower 32 bits of ms since epoch

    static func imuPacket(from motion: CMDeviceMotion) -> Data {
        var buf = Data(count: 22)

        // userAcceleration is already in g's (CoreMotion convention)
        let ax = Int16(clamping: Int64((motion.userAcceleration.x * 1000).rounded()))
        let ay = Int16(clamping: Int64((motion.userAcceleration.y * 1000).rounded()))
        let az = Int16(clamping: Int64((motion.userAcceleration.z * 1000).rounded()))

        // rotationRate is in rad/s; multiply by 5729.578 to get centi-deg/s
        let gx = Int16(clamping: Int64((motion.rotationRate.x * 5729.578).rounded()))
        let gy = Int16(clamping: Int64((motion.rotationRate.y * 5729.578).rounded()))
        let gz = Int16(clamping: Int64((motion.rotationRate.z * 5729.578).rounded()))

        // magneticField.field is in μT; pack directly as Int16
        let mx = Int16(clamping: Int64(motion.magneticField.field.x.rounded()))
        let my = Int16(clamping: Int64(motion.magneticField.field.y.rounded()))
        let mz = Int16(clamping: Int64(motion.magneticField.field.z.rounded()))

        let tsMsLow = UInt32(UInt64(Date().timeIntervalSince1970 * 1000) & 0xFFFFFFFF)

        buf.writeInt16LE(ax, at: 0);  buf.writeInt16LE(ay, at: 2);  buf.writeInt16LE(az, at: 4)
        buf.writeInt16LE(gx, at: 6);  buf.writeInt16LE(gy, at: 8);  buf.writeInt16LE(gz, at: 10)
        buf.writeInt16LE(mx, at: 12); buf.writeInt16LE(my, at: 14); buf.writeInt16LE(mz, at: 16)
        buf.writeUInt32LE(tsMsLow, at: 18)

        return buf
    }

    // MARK: - Ranging notification parser
    //
    // [0]      Uint8    anchor count  N
    // Per anchor (9 bytes, offset = 1 + i×9):
    //   [+0]   Uint8    anchor index
    //   [+1-4] Int32 LE range mm
    //   [+5-6] Int16 LE RSSI × 10 (dBm × 10)
    //   [+7-8] Uint16LE age ms

    static func parseRanging(_ data: Data) -> [AnchorInfo] {
        guard data.count >= 1 else { return [] }
        let count = Int(data[0])
        let now   = Date()
        var result: [AnchorInfo] = []

        for i in 0..<count {
            let base = 1 + i * 9
            guard base + 9 <= data.count else { break }

            let anchorID   = data[base]
            let rangeMM    = data.readInt32LE(at: base + 1)
            let rssiTimes10 = data.readInt16LE(at: base + 5)
            let ageMs      = data.readUInt16LE(at: base + 7)

            result.append(AnchorInfo(
                id:          anchorID,
                rangeMetres: Double(rangeMM) / 1000.0,
                rssiDBm:     Double(rssiTimes10) / 10.0,
                ageMs:       ageMs,
                receivedAt:  now
            ))
        }
        return result
    }
}

// MARK: - Data helpers (little-endian read / write)

private extension Data {
    mutating func writeInt16LE(_ value: Int16, at offset: Int) {
        let v = UInt16(bitPattern: value)
        self[offset]     = UInt8(v & 0xFF)
        self[offset + 1] = UInt8(v >> 8)
    }
    mutating func writeUInt16LE(_ value: UInt16, at offset: Int) {
        self[offset]     = UInt8(value & 0xFF)
        self[offset + 1] = UInt8(value >> 8)
    }
    mutating func writeInt32LE(_ value: Int32, at offset: Int) {
        let v = UInt32(bitPattern: value)
        self[offset]     = UInt8(v & 0xFF)
        self[offset + 1] = UInt8((v >> 8)  & 0xFF)
        self[offset + 2] = UInt8((v >> 16) & 0xFF)
        self[offset + 3] = UInt8((v >> 24) & 0xFF)
    }
    mutating func writeUInt32LE(_ value: UInt32, at offset: Int) {
        self[offset]     = UInt8(value & 0xFF)
        self[offset + 1] = UInt8((value >> 8)  & 0xFF)
        self[offset + 2] = UInt8((value >> 16) & 0xFF)
        self[offset + 3] = UInt8((value >> 24) & 0xFF)
    }

    func readInt16LE(at offset: Int) -> Int16 {
        let lo = UInt16(self[offset])
        let hi = UInt16(self[offset + 1])
        return Int16(bitPattern: lo | (hi << 8))
    }
    func readInt32LE(at offset: Int) -> Int32 {
        let b0 = UInt32(self[offset])
        let b1 = UInt32(self[offset + 1])
        let b2 = UInt32(self[offset + 2])
        let b3 = UInt32(self[offset + 3])
        return Int32(bitPattern: b0 | (b1 << 8) | (b2 << 16) | (b3 << 24))
    }
    func readUInt16LE(at offset: Int) -> UInt16 {
        UInt16(self[offset]) | (UInt16(self[offset + 1]) << 8)
    }
}

// MARK: - Safe integer clamping

private extension Int16 {
    init(clamping value: Int64) {
        self = Int16(max(Int64(Int16.min), min(Int64(Int16.max), value)))
    }
}
private extension Int32 {
    init(clamping value: Int64) {
        self = Int32(max(Int64(Int32.min), min(Int64(Int32.max), value)))
    }
}
private extension UInt16 {
    init(clamping value: Int64) {
        self = UInt16(max(Int64(0), min(Int64(UInt16.max), value)))
    }
}
private extension UInt8 {
    init(clamping value: Int64) {
        self = UInt8(max(Int64(0), min(Int64(UInt8.max), value)))
    }
}