path: root/IQ/IQUtils.swift

//
//  IQUtils.swift
//  PrySDR
//
//  Created by Jacky Jack on 21/01/2025.
//

import Accelerate
import ComplexModule

/// Convert from UInt8 to Float, naive implementation
/// - Parameters:
///   - arr: array of UInt8
/// - Returns: Return floats -0.996...0.996
///
func cnvU8toFloat32(_ arr: [UInt8]) -> [Float32] {
    
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    
    // 0..255 -> -0.996 ... 0.996
    // 0..127 -> -0.996 ... 0.0039
    // 128...255 -> 0.0039 ... 0.996
    for i in 0..<arr.count {
        out[i] = (Float(arr[i])-127.5)/128.0
    }
    
    return out
}

/// Convert from UInt8 to Float, naive implementation RtlSdr specfic
/// https://cgit.osmocom.org/gr-osmosdr/tree/lib/rtl/rtl_source_c.cc#n179
/// - Parameters:
///   - arr: array of UInt8
/// - Returns: Return floats -0.996...0.996
///
func cnvU8toFloat32_rtlsdr(_ arr: [UInt8]) -> [Float32] {
    
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    
    // 0..255 -> -1.0 ... 1.0
    for i in 0..<arr.count {
        out[i] = (Float(arr[i])-127.4)/128.0
    }
    
    return out
}


/// Convert from UInt8 to Float, using Accel framework
/// - Parameters:
///   - arr: array of UInt8
/// - Returns: Return floats -1.0...1.0
///
func cnvU8toFloat32_accel(_ arr: [UInt8]) -> [Float32] {
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    vDSP.convertElements(of: arr, to: &out)
    out = vDSP.add(-127.5, out)
    out = vDSP.divide(out, 128)
    return out
}

/// Convert from UInt8 to Float, using Accel framework specific to rtlsdr
/// - Parameters:
///   - arr: array of UInt8
/// - Returns: Return floats -1.0...1.0
///
func cnvU8toFloat32_accel_rtlsdr(_ arr: [UInt8]) -> [Float32] {
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    vDSP.convertElements(of: arr, to: &out)
    out = vDSP.add(-127.4, out)
    out = vDSP.divide(out, 128)
    return out
}

/// Convert from Int16 to Float, naive implementation for AirSpy
/// - Parameters:
///   - arr: array of Int16
/// - Returns: Return floats -0.996...0.996
///
func cnvI16toFloat32(_ arr: [Int16]) -> [Float32] {
    
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    
    // -32768..32767 -> -0.996 ... 0.996
    // -32768..0 -> -0.999 ... 0.0
    // 1...32767 -> 0.0 ... 0.999
    for i in 0..<arr.count {
        out[i] = (Float(arr[i])+0.5)/32768.0
    }
    
    return out
}

/// Convert from Int16 to Float, naive implementation for AirSpy
/// - Parameters:
///   - arr: array of Int16
/// - Returns: Return floats -0.996...0.996
///
func cnvU16toFloat32(_ arr: [UInt16]) -> [Float32] {
    
    print("!!!!")
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    
    // 0..65535 -> -0.999 ... 0.999
    // 0..32767 -> -0.999 ... 0.0
    // 32768...65535 -> 0.0 ... 0.999
    for i in 0..<arr.count {
        out[i] = ((Float(arr[i])+0.5)/32768.0) - 1.0
    }
    
    return out
}

/// Convert from Int16 to Float, naive implementation for AirSpy
/// - Parameters:
///   - arr: array of Int16
/// - Returns: Return floats -0.996...0.996
///
func cnvSC16Q11toFloat32(_ arr: [Int16]) -> [Float32] {
    
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    //print("!!!!")
    var out: [Float32] = .init(repeating: 0.0, count: arr.count)
    var cnt_overflow=0
    var cnt_underflow=0
    // -32768..32767 -> -0.996 ... 0.996
    // -32768..0 -> -0.999 ... 0.0
    // 1...32767 -> 0.0 ... 0.999
    for i in 0..<arr.count {
        //out[i] = (Float(arr[i])+0.5)/32768.0
        if arr[i] < 0 {
            out[i] = Float(arr[i])/2048.0
            if out[i] < -1.0 {
                cnt_underflow += 1
            }
        } else {
            out[i] = Float(arr[i])/2048.0
            if out[i] > 1.0 {
                //print("\(arr[i])->\(out[i])")
                cnt_overflow += 1
            }
        }
    }
    
    print("overflow: \(cnt_overflow) underflow: \(cnt_underflow)")
    
    return out
}

func cnvSC16Q11toFloat32_2(_ arr: [Int16]) -> [Float32] {
    
    //let iq = IQ(size: 8, bits: 8, sign: false, complex: true)
    //print("!!!!")
    let out: [Float32] = .init(repeating: 0.0, count: arr.count)
    let cnt_overflow=0
    let cnt_underflow=0
    // -32768..32767 -> -0.996 ... 0.996
    // -32768..0 -> -0.999 ... 0.0
    // 1...32767 -> 0.0 ... 0.999
    for i in 0..<arr.count {
        let data = arr[i]&0x7FF
        let sign = arr[i]&0x800
    }
    
    print("overflow: \(cnt_overflow) underflow: \(cnt_underflow)")
    
    return out
}


/// Convert Data stream to Int16 array
/// - Parameters:
///   - arr: array of data
/// - Returns: returns array of Int16
///
func asInt16<Input>(_ input: Input) -> AnySequence<Int16>
where
Input: Sequence,
Input.Element == UInt8
{
    let s = sequence(state: input.makeIterator()) { iter -> Int16? in
        guard
            let b1 = iter.next(),
            let b2 = iter.next()
        else {
            return nil
        }
        //do we care about endianess at this point?
        return Int16(b2) << 8 | Int16(b1)
    }
    return AnySequence(s)
}

/// Convert Data stream to Unt16 array
/// - Parameters:
///   - arr: array of data
/// - Returns: returns array of Int16
///
func asUInt16<Input>(_ input: Input) -> AnySequence<UInt16>
where
Input: Sequence,
Input.Element == UInt8
{
    let s = sequence(state: input.makeIterator()) { iter -> UInt16? in
        guard
            let b1 = iter.next(),
            let b2 = iter.next()
        else {
            return nil
        }
        //do we care about endianess at this point?
        return UInt16(b2) << 8 | UInt16(b1)
    }
    return AnySequence(s)
}



/// Convert Data stream to SC16Q11 specific to BladeRF
/// - Parameters:
///   - arr: array of data
/// - Returns: returns array of Int12, -4096...4096
///
func asSC16Q11<Input>(_ input: Input) -> AnySequence<Int16>
where
Input: Sequence,
Input.Element == UInt8
{
    let s = sequence(state: input.makeIterator()) { iter -> Int16? in
        guard
            let b1 = iter.next(),
            let b2 = iter.next()
        else {
            return nil
        }
        //sign
        print(b1)
        print(b2)
        let sign:UInt16 = (UInt16(b1)&0x8) >> 3
        print(sign)
        let val:Int16 = Int16(b1&0x7) << 8 | Int16(b2)
        //lower bound inclusive [-2048...2047]
        if (sign == 1) {
            return -1*(val+1)
        }
        return val
    }
    return AnySequence(s)
}

/// Convert F32 array to U8
/// - Parameters:
///   - arr: array of Float data
/// - Returns: returns array of UInt8
///
func cnvFloat32ToUInt8(_ arr: [Float32]) -> [UInt8] {
    var ret : [UInt8] = Array(repeating: UInt8(0), count: arr.count)
    for i in 0..<arr.count {
        var element = arr[i]
        if element > 1.0 {
            element = 1.0
        } else if element < -1.0 {
            element = -1.0
        }
        ret[i] = UInt8(127.0*(element+1.0))
    }
    return ret
}

func cnvFloat32ToCUInt8(_ el: Float32) -> UInt8 {
    var ret:Float32 = el
    if el > 1.0 {
        ret = 1.0
    } else if el < -1.0 {
        ret = 1.0
    }
    return UInt8(127.0*(ret+1.0))
}

/// Convert CF32 array to U8
/// - Parameters:
///   - arr: array of Compex Float data
/// - Returns: returns array of IQ UInt8
///
func cnvFloat32ToCUInt8(_ arr: [Complex<Float32>]) -> [UInt8] {
    var ret : [UInt8] = Array(repeating: UInt8(0), count: arr.count*2)
    for i in 0..<arr.count {
        let element = arr[i]
        ret[i*2]   = cnvFloat32ToCUInt8(element.real)
        ret[i*2+1] = cnvFloat32ToCUInt8(element.imaginary)
    }
    return ret
}

/// Convert F32 array to I8
/// - Parameters:
///   - arr: array of Float data
/// - Returns: returns array of Int8
///
func cnvFloat32ToInt8(_ arr: [Float32]) -> [Int8] {
    var ret : [Int8] = Array(repeating: Int8(0), count: arr.count)
    for i in 0..<arr.count {
        var element = arr[i]
        if element > 1.0 {
            element = 1.0
        } else if element < -1.0 {
            element = -1.0
        }
        if (element>0.0) {
            ret[i] = Int8(127.0*(element))
        } else {
            ret[i] = Int8(-128.0*(-element))
        }
    }
    return ret
}

func cnvFloat32ToCInt8(_ el: Float32) -> Int8 {
    var ret:Float32 = el
    if ret > 1.0 {
        ret = 1.0
    } else if ret < -1.0 {
        ret = -1.0
    }
    if (ret>0.0) {
        return Int8(127.0*(ret))
    } else {
        return Int8(128.0*(ret))
    }
}

func cnvFloat32ToCInt8(_ arr: [Complex<Float32>]) -> [Int8] {
    var ret : [Int8] = Array(repeating: Int8(0), count: arr.count*2)
    for i in 0..<arr.count {
        let element = arr[i]
        ret[i*2]   = cnvFloat32ToCInt8(element.real)
        ret[i*2+1] = cnvFloat32ToCInt8(element.imaginary)
    }
    return ret
}

/// Conver F32 array to U16
/// - Parameters:
///   - arr: array of Float data
/// - Returns: returns array of UInt16
///
func cnvFloat32ToUInt16(_ arr: [Float32]) -> [UInt16] {
    var ret : [UInt16] = Array(repeating: UInt16(0), count: arr.count)
    for i in 0..<arr.count {
        var element = arr[i]
        if element > 1.0 {
            element = 1.0
        } else if element < -1.0 {
            element = -1.0
        }
        ret[i] = UInt16(32767*(element+1.0))
    }
    return ret
}

func cnvFloat32ToCUInt16(_ el: Float32) -> UInt16 {
    var ret:Float32 = el
    if el > 1.0 {
        ret = 1.0
    } else if el < -1.0 {
        ret = -1.0
    }
    return UInt16(32767*(ret+1.0))
}

func cnvFloat32ToCUInt16(_ arr: [Complex<Float32>]) -> [UInt16] {
    var ret : [UInt16] = Array(repeating: UInt16(0), count: arr.count*2)
    for i in 0..<arr.count {
        let element = arr[i]
        ret[i*2] = cnvFloat32ToCUInt16(element.real)
        ret[i*2+1] = cnvFloat32ToCUInt16(element.imaginary)
    }
    return ret
}

/// Conver F32 array to I16
/// - Parameters:
///   - arr: array of Float data
/// - Returns: returns array of Int16
///
func cnvFloat32ToInt16(_ arr: [Float32]) -> [Int16] {
    var ret : [Int16] = Array(repeating: Int16(0), count: arr.count)
    for i in 0..<arr.count {
        var element = arr[i]
        if element > 1.0 {
            element = 1.0
        } else if element < -1.0 {
            element = -1.0
        }
        if (element>0.0) {
            ret[i] = Int16(32767.0*(element))
        } else {
            ret[i] = Int16(-32768.0*(-element))
        }
    }
    return ret
}

func cnvFloat32ToCInt16(_ el: Float32) -> Int16 {
    var ret:Float32 = el
    
    if el > 1.0 {
        ret = 1.0
    } else if el < -1.0 {
        ret = -1.0
    }
    
    if (ret>0.0) {
        return Int16(32767.0*(ret))
    } else {
        return Int16(-32768.0*(-ret))
    }
}

func cnvFloat32ToCInt16(_ arr: [Complex<Float32>]) -> [Int16] {
    var ret : [Int16] = Array(repeating: Int16(0), count: arr.count*2)
    for i in 0..<arr.count {
        let element = arr[i]
        ret[i*2] = cnvFloat32ToCInt16(element.real)
        ret[i*2+1] =  cnvFloat32ToCInt16(element.imaginary)
    }
    return ret
}

/// Conver F32 array to S16Q11
/// - Parameters:
///   - arr: array of Float data
/// - Returns: returns array of S16Q11
///
func cnvFloat32ToS16Q11(_ arr: [Float32]) -> [Int16] {
    var ret : [Int16] = Array(repeating: Int16(0), count: arr.count)
    for i in 0..<arr.count {
        var element = arr[i]
        if element > 1.0 {
            element = 1.0
        } else if element < -1.0 {
            element = -1.0
        }
        if (element>0.0) {
            ret[i] = Int16(2048.0*(element))
        } else {
            ret[i] = Int16(-2048.0*(-element))
        }
    }
    return ret
}


func cnvFloat32ToS16Q11(_ el: Float32) -> Int16 {
    var ret:Float32 = el
    if el > 1.0 {
        ret = 1.0
    } else if el < -1.0 {
        ret = -1.0
    }
    if (ret>0.0) {
        return Int16(2048.0*(ret))
    }
    return Int16(2048.0*(ret))
}

/// Conver F32 array to SC16Q11
/// - Parameters:
///   - arr: array of Complex<Float32> data
/// - Returns: returns array of IQ S16
///
func cnvFloat32ToS16Q11(_ arr: [Complex<Float32>]) -> [Int16] {
    var ret : [Int16] = Array(repeating: Int16(0), count: arr.count*2)
    for i in 0..<arr.count {
        let element = arr[i]
        ret[i*2] = cnvFloat32ToS16Q11(element.real)
        ret[i*2+1] = cnvFloat32ToS16Q11(element.imaginary)
    }
    return ret
}