/// EIS4 SysEx Protocol — manufacturer ID 0x7D (non-commercial)
/// Port of cue/src/protocol.rs

import Foundation

// MARK: - Constants

let sysexMfr: UInt8 = 0x7D

// ESP32 -> Cue
let RSP_SWEEP_START:  UInt8 = 0x01
let RSP_DATA_POINT:   UInt8 = 0x02
let RSP_SWEEP_END:    UInt8 = 0x03
let RSP_CONFIG:       UInt8 = 0x04
let RSP_LSV_START:    UInt8 = 0x05
let RSP_LSV_POINT:    UInt8 = 0x06
let RSP_LSV_END:      UInt8 = 0x07
let RSP_AMP_START:    UInt8 = 0x08
let RSP_AMP_POINT:    UInt8 = 0x09
let RSP_AMP_END:      UInt8 = 0x0A
let RSP_CL_START:     UInt8 = 0x0B
let RSP_CL_POINT:     UInt8 = 0x0C
let RSP_CL_RESULT:    UInt8 = 0x0D
let RSP_CL_END:       UInt8 = 0x0E
let RSP_PH_RESULT:    UInt8 = 0x0F
let RSP_TEMP:         UInt8 = 0x10
let RSP_REF_FRAME:    UInt8 = 0x20
let RSP_REF_LP_RANGE: UInt8 = 0x21
let RSP_REFS_DONE:    UInt8 = 0x22
let RSP_REF_STATUS:   UInt8 = 0x23

// Cue -> ESP32
let CMD_SET_SWEEP:     UInt8 = 0x10
let CMD_SET_RTIA:      UInt8 = 0x11
let CMD_SET_RCAL:      UInt8 = 0x12
let CMD_START_SWEEP:   UInt8 = 0x13
let CMD_GET_CONFIG:    UInt8 = 0x14
let CMD_SET_ELECTRODE: UInt8 = 0x15
let CMD_GET_TEMP:      UInt8 = 0x17
let CMD_START_LSV:     UInt8 = 0x20
let CMD_START_AMP:     UInt8 = 0x21
let CMD_STOP_AMP:      UInt8 = 0x22
let CMD_START_CL:      UInt8 = 0x23
let CMD_START_PH:      UInt8 = 0x24
let CMD_START_CLEAN:   UInt8 = 0x25
let CMD_START_REFS:    UInt8 = 0x30
let CMD_GET_REFS:      UInt8 = 0x31
let CMD_CLEAR_REFS:    UInt8 = 0x32

// MARK: - 7-bit MIDI encoding

/// Encode an IEEE 754 float into 5 MIDI-safe bytes.
/// Byte 0: mask of high bits from each original byte.
/// Bytes 1-4: original bytes with bit 7 stripped.
func encodeFloat(_ val: Float) -> [UInt8] {
    var v = val
    let p = withUnsafeBytes(of: &v) { Array($0) }
    let mask: UInt8 = ((p[0] >> 7) & 1)
                    | ((p[1] >> 6) & 2)
                    | ((p[2] >> 5) & 4)
                    | ((p[3] >> 4) & 8)
    return [mask, p[0] & 0x7F, p[1] & 0x7F, p[2] & 0x7F, p[3] & 0x7F]
}

/// Decode 5 MIDI-safe bytes back into an IEEE 754 float.
func decodeFloat(_ d: [UInt8], at offset: Int = 0) -> Float {
    let m = d[offset]
    let b0 = d[offset + 1] | ((m & 1) << 7)
    let b1 = d[offset + 2] | ((m & 2) << 6)
    let b2 = d[offset + 3] | ((m & 4) << 5)
    let b3 = d[offset + 4] | ((m & 8) << 4)
    var val: Float = 0
    withUnsafeMutableBytes(of: &val) { buf in
        buf[0] = b0; buf[1] = b1; buf[2] = b2; buf[3] = b3
    }
    return val
}

/// Encode a UInt16 into 3 MIDI-safe bytes.
func encodeU16(_ val: UInt16) -> [UInt8] {
    var v = val
    let p = withUnsafeBytes(of: &v) { Array($0) }
    let mask: UInt8 = ((p[0] >> 7) & 1) | ((p[1] >> 6) & 2)
    return [mask, p[0] & 0x7F, p[1] & 0x7F]
}

/// Decode 3 MIDI-safe bytes back into a UInt16.
func decodeU16(_ d: [UInt8], at offset: Int = 0) -> UInt16 {
    let m = d[offset]
    let b0 = d[offset + 1] | ((m & 1) << 7)
    let b1 = d[offset + 2] | ((m & 2) << 6)
    var val: UInt16 = 0
    withUnsafeMutableBytes(of: &val) { buf in
        buf[0] = b0; buf[1] = b1
    }
    return val
}

// MARK: - Message enum

enum EisMessage {
    case sweepStart(numPoints: UInt16, freqStart: Float, freqStop: Float)
    case dataPoint(index: UInt16, point: EisPoint)
    case sweepEnd
    case config(EisConfig)
    case lsvStart(numPoints: UInt16, vStart: Float, vStop: Float)
    case lsvPoint(index: UInt16, point: LsvPoint)
    case lsvEnd
    case ampStart(vHold: Float)
    case ampPoint(index: UInt16, point: AmpPoint)
    case ampEnd
    case clStart(numPoints: UInt16)
    case clPoint(index: UInt16, point: ClPoint)
    case clResult(ClResult)
    case clEnd
    case phResult(PhResult)
    case temperature(Float)
    case refFrame(mode: UInt8, rtiaIdx: UInt8)
    case refLpRange(mode: UInt8, lowIdx: UInt8, highIdx: UInt8)
    case refsDone
    case refStatus(hasRefs: Bool)
}

// MARK: - Response parser

/// Parse a SysEx payload (after BLE MIDI unwrapping).
/// Input: [0x7D, RSP_ID, ...payload_bytes...]
func parseSysex(_ data: [UInt8]) -> EisMessage? {
    guard data.count >= 2, data[0] == sysexMfr else { return nil }
    let p = Array(data.dropFirst(2))

    switch data[1] {

    case RSP_SWEEP_START where p.count >= 13:
        return .sweepStart(
            numPoints: decodeU16(p, at: 0),
            freqStart: decodeFloat(p, at: 3),
            freqStop:  decodeFloat(p, at: 8)
        )

    case RSP_DATA_POINT where p.count >= 28:
        let ext = p.count >= 53
        return .dataPoint(
            index: decodeU16(p, at: 0),
            point: EisPoint(
                freqHz:        decodeFloat(p, at: 3),
                magOhms:       decodeFloat(p, at: 8),
                phaseDeg:      decodeFloat(p, at: 13),
                zReal:         decodeFloat(p, at: 18),
                zImag:         decodeFloat(p, at: 23),
                rtiaMagBefore: ext ? decodeFloat(p, at: 28) : 0,
                rtiaMagAfter:  ext ? decodeFloat(p, at: 33) : 0,
                revMag:        ext ? decodeFloat(p, at: 38) : 0,
                revPhase:      ext ? decodeFloat(p, at: 43) : 0,
                pctErr:        ext ? decodeFloat(p, at: 48) : 0
            )
        )

    case RSP_SWEEP_END:
        return .sweepEnd

    case RSP_CONFIG where p.count >= 16:
        return .config(EisConfig(
            freqStart: decodeFloat(p, at: 0),
            freqStop:  decodeFloat(p, at: 5),
            ppd:       decodeU16(p, at: 10),
            rtia:      Rtia(rawValue: p[13]) ?? .r5K,
            rcal:      Rcal(rawValue: p[14]) ?? .r3K,
            electrode: Electrode(rawValue: p[15]) ?? .fourWire
        ))

    case RSP_LSV_START where p.count >= 13:
        return .lsvStart(
            numPoints: decodeU16(p, at: 0),
            vStart:    decodeFloat(p, at: 3),
            vStop:     decodeFloat(p, at: 8)
        )

    case RSP_LSV_POINT where p.count >= 13:
        return .lsvPoint(
            index: decodeU16(p, at: 0),
            point: LsvPoint(
                vMv: decodeFloat(p, at: 3),
                iUa: decodeFloat(p, at: 8)
            )
        )

    case RSP_LSV_END:
        return .lsvEnd

    case RSP_AMP_START where p.count >= 5:
        return .ampStart(vHold: decodeFloat(p, at: 0))

    case RSP_AMP_POINT where p.count >= 13:
        return .ampPoint(
            index: decodeU16(p, at: 0),
            point: AmpPoint(
                tMs: decodeFloat(p, at: 3),
                iUa: decodeFloat(p, at: 8)
            )
        )

    case RSP_AMP_END:
        return .ampEnd

    case RSP_CL_START where p.count >= 3:
        return .clStart(numPoints: decodeU16(p, at: 0))

    case RSP_CL_POINT where p.count >= 14:
        return .clPoint(
            index: decodeU16(p, at: 0),
            point: ClPoint(
                tMs:   decodeFloat(p, at: 3),
                iUa:   decodeFloat(p, at: 8),
                phase: p[13]
            )
        )

    case RSP_CL_RESULT where p.count >= 10:
        return .clResult(ClResult(
            iFreeUa:  decodeFloat(p, at: 0),
            iTotalUa: decodeFloat(p, at: 5)
        ))

    case RSP_CL_END:
        return .clEnd

    case RSP_PH_RESULT where p.count >= 15:
        return .phResult(PhResult(
            vOcpMv: decodeFloat(p, at: 0),
            ph:     decodeFloat(p, at: 5),
            tempC:  decodeFloat(p, at: 10)
        ))

    case RSP_TEMP where p.count >= 5:
        return .temperature(decodeFloat(p, at: 0))

    case RSP_REF_FRAME where p.count >= 2:
        return .refFrame(mode: p[0], rtiaIdx: p[1])

    case RSP_REF_LP_RANGE where p.count >= 3:
        return .refLpRange(mode: p[0], lowIdx: p[1], highIdx: p[2])

    case RSP_REFS_DONE:
        return .refsDone

    case RSP_REF_STATUS where p.count >= 1:
        return .refStatus(hasRefs: p[0] != 0)

    default:
        return nil
    }
}

// MARK: - Command builders

func buildSysexSetSweep(freqStart: Float, freqStop: Float, ppd: UInt16) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_SET_SWEEP]
    sx.append(contentsOf: encodeFloat(freqStart))
    sx.append(contentsOf: encodeFloat(freqStop))
    sx.append(contentsOf: encodeU16(ppd))
    sx.append(0xF7)
    return sx
}

func buildSysexSetRtia(_ rtia: Rtia) -> [UInt8] {
    [0xF0, sysexMfr, CMD_SET_RTIA, rtia.rawValue, 0xF7]
}

func buildSysexSetRcal(_ rcal: Rcal) -> [UInt8] {
    [0xF0, sysexMfr, CMD_SET_RCAL, rcal.rawValue, 0xF7]
}

func buildSysexSetElectrode(_ e: Electrode) -> [UInt8] {
    [0xF0, sysexMfr, CMD_SET_ELECTRODE, e.rawValue, 0xF7]
}

func buildSysexStartSweep() -> [UInt8] {
    [0xF0, sysexMfr, CMD_START_SWEEP, 0xF7]
}

func buildSysexGetConfig() -> [UInt8] {
    [0xF0, sysexMfr, CMD_GET_CONFIG, 0xF7]
}

func buildSysexStartLsv(vStart: Float, vStop: Float, scanRate: Float, lpRtia: LpRtia) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_START_LSV]
    sx.append(contentsOf: encodeFloat(vStart))
    sx.append(contentsOf: encodeFloat(vStop))
    sx.append(contentsOf: encodeFloat(scanRate))
    sx.append(lpRtia.rawValue)
    sx.append(0xF7)
    return sx
}

func buildSysexStartAmp(vHold: Float, intervalMs: Float, durationS: Float, lpRtia: LpRtia) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_START_AMP]
    sx.append(contentsOf: encodeFloat(vHold))
    sx.append(contentsOf: encodeFloat(intervalMs))
    sx.append(contentsOf: encodeFloat(durationS))
    sx.append(lpRtia.rawValue)
    sx.append(0xF7)
    return sx
}

func buildSysexStopAmp() -> [UInt8] {
    [0xF0, sysexMfr, CMD_STOP_AMP, 0xF7]
}

func buildSysexStartCl(
    vCond: Float, tCondMs: Float, vFree: Float, vTotal: Float,
    tDepMs: Float, tMeasMs: Float, lpRtia: LpRtia
) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_START_CL]
    sx.append(contentsOf: encodeFloat(vCond))
    sx.append(contentsOf: encodeFloat(tCondMs))
    sx.append(contentsOf: encodeFloat(vFree))
    sx.append(contentsOf: encodeFloat(vTotal))
    sx.append(contentsOf: encodeFloat(tDepMs))
    sx.append(contentsOf: encodeFloat(tMeasMs))
    sx.append(lpRtia.rawValue)
    sx.append(0xF7)
    return sx
}

func buildSysexGetTemp() -> [UInt8] {
    [0xF0, sysexMfr, CMD_GET_TEMP, 0xF7]
}

func buildSysexStartPh(stabilizeS: Float) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_START_PH]
    sx.append(contentsOf: encodeFloat(stabilizeS))
    sx.append(0xF7)
    return sx
}

func buildSysexStartClean(vMv: Float, durationS: Float) -> [UInt8] {
    var sx: [UInt8] = [0xF0, sysexMfr, CMD_START_CLEAN]
    sx.append(contentsOf: encodeFloat(vMv))
    sx.append(contentsOf: encodeFloat(durationS))
    sx.append(0xF7)
    return sx
}

func buildSysexStartRefs() -> [UInt8] {
    [0xF0, sysexMfr, CMD_START_REFS, 0xF7]
}

func buildSysexGetRefs() -> [UInt8] {
    [0xF0, sysexMfr, CMD_GET_REFS, 0xF7]
}

func buildSysexClearRefs() -> [UInt8] {
    [0xF0, sysexMfr, CMD_CLEAR_REFS, 0xF7]
}