import SoapySDR
import numpy as np

from noptica import InductionHeater, Stabilizer, PositionTracker


def main():
    freq_sample = 5e6
    freq_base = 1086e6
    bufsize = 4096

    induction = InductionHeater("/dev/ttyUSB0", 430e3, 445e3)
    induction.start()
    try:
        stabilizer = Stabilizer(freq_sample, 0.4, 1088.3e6 - freq_base, 200e-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)

        samples_ref = np.array([0]*bufsize, np.complex64)
        samples_meas = np.array([0]*bufsize, np.complex64)
        rx_stream = sdr.setupStream(SoapySDR.SOAPY_SDR_RX, SoapySDR.SOAPY_SDR_CF32, [0, 1])
        try:
            sdr.activateStream(rx_stream)
            try:
                stabilizer_throttle = 0
                while True:
                    sr = sdr.readStream(rx_stream, [samples_ref, samples_meas], bufsize)
                    if sr.ret != bufsize:
                        print("SDR sampling error")
                        return

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

                    position, leakage = position_tracker.input(samples_ref, samples_meas)
                    print(np.sum(position)/len(position), leakage)
            finally:
                sdr.deactivateStream(rx_stream)
        finally:
            sdr.closeStream(rx_stream)
    finally:
        induction.stop()


if __name__ == "__main__":
    main()