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

import Accelerate

/// 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("!!!!")
    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 {
        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)
}

/// Conver 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
}

/// Conver 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
}

/// 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
}

/// 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
}