from math import ceil

import SoapySDR
import numpy as np
import gc

from noptica import *
from gui import GUI


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

    freq_sample = 1e6
    freq_base = 1088230e3
    freq_ref = 1088100e3
    block_size = 4096
    throttle_factor = ceil(freq_sample/125e3)

    # note: image band still goes through
    freq_ref_abs = np.abs(freq_ref-freq_base)
    ref_filter = LinearPhaseFilter(127, [freq_ref_abs-15e3, freq_ref_abs+15e3], pass_zero=False, fs=freq_sample)
    meas_delay = Delay(block_size, ref_filter.delay())

    gui = GUI(freq_sample, freq_base, block_size)
    try:
        induction = InductionHeater("/dev/ttyUSB0", 350e3, 445e3)
        induction.start()
        try:
            def stabilizer_cb(spectrum, peak_freq, locked, tuning):
                gui.update_ref(spectrum, peak_freq, locked)
                induction.set(tuning)

            stabilizer = Stabilizer(freq_sample, block_size, freq_ref-freq_base, stabilizer_cb)
            position_tracker = PositionTracker()

            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)
                sdr.setGain(SoapySDR.SOAPY_SDR_RX, channel, 55.0)

            buf_sdr = BufferedSDR(sdr, [0, 1], block_size, 32)
            buf_sdr.start()
            try:
                throttle = 0
                position_acc = 0.0
                while True:
                    buffers = buf_sdr.get()
                    try:
                        samples_ref, samples_meas = buffers

                        # Throttle certain things to avoid overflows due to the limited speed of
                        # the MHS5200A serial interface and GUI plotting.
                        throttle += 1
                        if throttle == throttle_factor:
                            throttle = 0

                        if throttle == 0:
                            stabilizer.input(samples_ref)
                            gui.update_meas(samples_meas)

                        if stabilizer.locked():
                            position = position_tracker.input(ref_filter.input(samples_ref), meas_delay.input(samples_meas))
                            position_acc += np.mean(position)
                            if throttle == 0:
                                gui.update_position(position_acc/throttle_factor)
                                position_acc = 0.0
                        else:
                            position_tracker.reset()
                            position_acc = 0.0
                    finally:
                        buf_sdr.dispose(buffers)
            finally:
                buf_sdr.stop()
        finally:
            induction.stop()
    finally:
        gui.close()


if __name__ == "__main__":
    main()