import SoapySDR
import numpy as np
from scipy.signal import blackmanharris

from stabilizer import InductionHeater

fs = 5e6
base_freq = 1086e6
threshold = 0.4
target_freq = 1088.3e6
k = 200e-6

def parabolic(f, x):
    xv = 1/2. * (f[x-1] - f[x+1]) / (f[x-1] - 2 * f[x] + f[x+1]) + x
    yv = f[x] - 1/4. * (f[x-1] - f[x+1]) * (xv - x)
    return (xv, yv)


def fmt_range(x, m):
    pos = int(round(x*80/m))
    return " "*pos + "*"


def main():
    induction = InductionHeater("/dev/ttyUSB0", 430e3, 445e3)
    induction.start()
    try:
        sdr = SoapySDR.Device()
        samples = np.array([0]*4096, np.complex64)

        sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, 0, fs)
        sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, 0, base_freq)

        rxStream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32)
        sdr.activateStream(rxStream)

        decimation = 0

        try:
            while True:
                sr = sdr.readStream(rxStream, [samples], len(samples))
                if sr.ret != len(samples):
                    print("!")
                    continue

                freqs = np.abs(np.fft.fft(samples*blackmanharris(len(samples)))[0:len(samples)//2])
                for i in range(len(freqs)//100):
                    freqs[i] = 0
                    freqs[-i] = 0

                # https://gist.github.com/endolith/255291
                i = np.argmax(freqs)
                true_i = parabolic(freqs, i)[0]
                freq = 0.5 * fs * true_i / len(freqs)
                amplitude = freqs[i]

                if amplitude > threshold:
                    actual_freq = base_freq + freq
                    print("{:8.3f} {:6.2f} {}".format(actual_freq/1e6, amplitude, fmt_range(freq, fs/2)))
                    induction_amt = (actual_freq - target_freq)*k
                else:
                    print("<no signal>", amplitude)
                    induction_amt = 0.0

                decimation += 1
                if decimation == 30:
                    induction.set(induction_amt)
                    decimation = 0
        finally:
            sdr.deactivateStream(rxStream)
            sdr.closeStream(rxStream)
    finally:
        induction.stop()


if __name__ == "__main__":
    main()