import SoapySDR
import numpy as np
import gc

from noptica import *


def main():
    # For this PoC code, a small memory leak is less harmful than random overflows.
    gc.disable()

    freq_sample = 5e6
    freq_base = 1086e6
    bufsize = 4096

    induction = InductionHeater("/dev/ttyUSB0", 350e3, 445e3)
    induction.start()
    try:
        stabilizer = Stabilizer(freq_sample, 0.4, 1088.1e6 - freq_base, 50e-6, induction)
        position_tracker = PositionTracker(int(0.1*freq_sample/bufsize))

        sdr = SoapySDR.Device()
        for channel in range(2):
            sdr.setSampleRate(SoapySDR.SOAPY_SDR_RX, channel, freq_sample)
            sdr.setFrequency(SoapySDR.SOAPY_SDR_RX, channel, freq_base)

        buf_sdr = BufferedSDR(sdr, [0, 1], bufsize, 256)
        buf_sdr.start()
        try:
            stabilizer_throttle = 0
            while True:
                buffers = buf_sdr.get()
                try:
                    samples_ref, samples_meas = buffers

                    # We can't update faster than the MHS5200A serial interface
                    stabilizer_throttle += 1
                    if stabilizer_throttle == 40:
                        stabilizer.input(samples_ref)
                        stabilizer_throttle = 0

                    position, leakage = position_tracker.input(samples_ref, samples_meas)
                    print(np.sum(position)/len(position), leakage)
                finally:
                    buf_sdr.dispose(buffers)
        finally:
            buf_sdr.stop()
    finally:
        induction.stop()


if __name__ == "__main__":
    main()