#include <HW_HAL.hpp>
#include <VNA.hpp>
#include "Si5351C.hpp"
#include "max2871.hpp"
#include "main.h"
#include "delay.hpp"
#include "FPGA/FPGA.hpp"
#include <complex>
#include "Exti.hpp"
#include "Hardware.hpp"
#include "Communication.h"
#include "FreeRTOS.h"
#include "task.h"
#include "Util.hpp"
#include "usb.h"

#define LOG_LEVEL	LOG_LEVEL_INFO
#define LOG_MODULE	"VNA"
#include "Log.h"

static Protocol::SweepSettings settings;
static uint16_t pointCnt;
static bool excitingPort1;
static Protocol::Datapoint data;
static bool active = false;
static Si5351C::DriveStrength fixedPowerLowband;
static bool adcShifted;
static uint32_t actualBandwidth;

static constexpr uint8_t sourceHarmonic = 5;
static constexpr uint8_t LOHarmonic = 3;

using IFTableEntry = struct {
	uint16_t pointCnt;
	uint8_t clkconfig[8];
};

using ADCTableEntry = struct {
	uint16_t pointCnt;
	uint8_t adcPrescaler;
};

static constexpr uint16_t IFTableNumEntries = 500;
static IFTableEntry IFTable[IFTableNumEntries];
static uint16_t IFTableIndexCnt = 0;

static constexpr uint16_t ADCTableNumEntries = 500;
static ADCTableEntry ADCTable[ADCTableNumEntries];
static uint16_t ADCTableIndexCnt = 0;

static constexpr float alternativeSamplerate = 914285.7143f;
static constexpr uint8_t alternativePrescaler = 102400000UL / alternativeSamplerate;
static_assert(alternativePrescaler * alternativeSamplerate == 102400000UL, "alternative ADCSamplerate can not be reached exactly");
static constexpr uint16_t alternativePhaseInc = 4096 * HW::IF2 / alternativeSamplerate;
static_assert(alternativePhaseInc * alternativeSamplerate == 4096 * HW::IF2, "DFT can not be computed for 2.IF when using alternative samplerate");

// Constants for USB buffer overflow prevention
static constexpr uint16_t maxPointsBetweenHalts = 40;
static constexpr uint32_t reservedUSBbuffer = maxPointsBetweenHalts * (sizeof(Protocol::Datapoint) + 8 /*USB packet overhead*/);

using namespace HWHAL;

bool VNA::Setup(Protocol::SweepSettings s) {
	VNA::Stop();
	vTaskDelay(5);
	HW::SetMode(HW::Mode::VNA);
	if(s.excitePort1 == 0 && s.excitePort2 == 0) {
		// both ports disabled, nothing to do
		HW::SetIdle();
		active = false;
		return false;
	}
	settings = s;
	// Abort possible active sweep first
	FPGA::SetMode(FPGA::Mode::FPGA);
	uint16_t points = settings.points <= FPGA::MaxPoints ? settings.points : FPGA::MaxPoints;
	// Configure sweep
	FPGA::SetNumberOfPoints(points);
	uint32_t samplesPerPoint = (HW::ADCSamplerate / s.if_bandwidth);
	// round up to next multiple of 16 (16 samples are spread across 5 IF2 periods)
	if(samplesPerPoint%16) {
		samplesPerPoint += 16 - samplesPerPoint%16;
	}
	actualBandwidth = HW::ADCSamplerate / samplesPerPoint;
	// has to be one less than actual number of samples
	FPGA::SetSamplesPerPoint(samplesPerPoint);

	// reset unlevel flag if it was set from a previous sweep/mode
	HW::SetOutputUnlevel(false);
	// Start with average level
	auto cdbm = (s.cdbm_excitation_start + s.cdbm_excitation_stop) / 2;
	// correct for port 1, assumes port 2 is identical
	auto centerFreq = (s.f_start + s.f_stop) / 2;
	// force calculation of amplitude setting for PLL, even with lower frequencies
	if(centerFreq < HW::BandSwitchFrequency) {
		centerFreq = HW::BandSwitchFrequency;
	}
	auto amplitude = HW::GetAmplitudeSettings(cdbm, centerFreq, true, false);
	if(amplitude.unlevel) {
		HW::SetOutputUnlevel(true);
	}

	uint8_t fixedAttenuatorHighband = amplitude.attenuator;
	Source.SetPowerOutA(amplitude.highBandPower, true);

	// amplitude calculation for lowband
	amplitude = HW::GetAmplitudeSettings(cdbm, HW::BandSwitchFrequency / 2, true, false);
	if(amplitude.unlevel) {
		HW::SetOutputUnlevel(true);
	}
	uint8_t fixedAttenuatorLowband = amplitude.attenuator;
	fixedPowerLowband = amplitude.lowBandPower;

	FPGA::WriteMAX2871Default(Source.GetRegisters());

	uint32_t last_LO2 = HW::IF1 - HW::IF2;
	Si5351.SetCLK(SiChannel::Port1LO2, last_LO2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
	Si5351.SetCLK(SiChannel::Port2LO2, last_LO2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
	Si5351.SetCLK(SiChannel::RefLO2, last_LO2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
	Si5351.ResetPLL(Si5351C::PLL::B);

	IFTableIndexCnt = 0;
	ADCTableIndexCnt = 0;
	uint8_t last_ADC_prescaler = HW::ADCprescaler;

	bool last_lowband = false;

	uint16_t pointsWithoutHalt = 0;

	// Transfer PLL configuration to FPGA
	for (uint16_t i = 0; i < points; i++) {
		bool harmonic_mixing = false;
		uint64_t freq = s.f_start + (s.f_stop - s.f_start) * i / (points - 1);
		int16_t power = s.cdbm_excitation_start + (s.cdbm_excitation_stop - s.cdbm_excitation_start) * i / (points - 1);
		freq = Cal::FrequencyCorrectionToDevice(freq);

		if(freq > 6000000000ULL) {
			harmonic_mixing = true;
		}

		// SetFrequency only manipulates the register content in RAM, no SPI communication is done.
		// No mode-switch of FPGA necessary here.

		bool needs_halt = false;
		uint64_t actualSourceFreq;
		bool lowband = false;
		if (freq < HW::BandSwitchFrequency) {
			needs_halt = true;
			lowband = true;
			actualSourceFreq = freq;
		} else {
			uint64_t srcFreq = freq;
			if(harmonic_mixing) {
				srcFreq /= sourceHarmonic;
			}
			Source.SetFrequency(srcFreq);
			actualSourceFreq = Source.GetActualFrequency();
			if(harmonic_mixing) {
				actualSourceFreq *= sourceHarmonic;
			}
		}
		if (last_lowband && !lowband) {
			// additional halt before first highband point to enable highband source
			needs_halt = true;
		}

		uint64_t LOFreq = freq + HW::IF1;

		// check for collision between IF and point frequency
		if(s.suppressPeaks && IFTableIndexCnt < IFTableNumEntries - 1) {
			static constexpr int8_t max_multiple = 3;
			bool needs_alternate_IF = false;
			for(int8_t i=-max_multiple;i<=max_multiple;i++) {
				if(i==0) {
					continue;
				}
				for(int8_t j=-max_multiple;j<=max_multiple;j++) {
					if(j==0) {
						continue;
					}
					float m_IF, m_freq;
					if(i<0) {
						m_IF = (float) HW::IF1 / -i;
					} else {
						m_IF = HW::IF1 * i;
					}
					if(j<0) {
						m_freq = (float) freq / -j;
					} else {
						m_freq = freq * j;
					}
					float ratio = m_IF / m_freq;
					if(ratio >= 0.97f && ratio <= 1.03f) {
						needs_alternate_IF = true;
						break;
					}
				}
				if(needs_alternate_IF) {
					break;
				}
			}
			if(needs_alternate_IF) {
				LOFreq += 2150000;
			}
		}

//		if(s.suppressPeaks && IFTableIndexCnt < IFTableNumEntries - 1) {
//			bool ADC_alias;
//			static constexpr uint64_t max_LO_shift = 2000000;
//			static constexpr uint64_t LO_shift_inc = 71777;
//			static constexpr uint32_t max_mixing_freq = 30000000;
//			do {
//				ADC_alias = false;
//				static uint8_t max_harmonic_LO1 = 5;
//				static uint8_t max_harmonic_LO2 = 97;
//				for (uint64_t harmonic_1LO = 1; harmonic_1LO <= max_harmonic_LO1;
//						harmonic_1LO+=2) {
//					for (uint64_t harmonic_2LO = 1; harmonic_2LO <= max_harmonic_LO2;
//							harmonic_2LO+=2) {
//						int64_t harmonic_freq_1LO = harmonic_1LO * LOFreq;
//						int64_t harmonic_freq_2LO = harmonic_2LO * (HW::IF1 - HW::IF2);
//
//						if (harmonic_freq_2LO > harmonic_freq_1LO + max_mixing_freq) {
//							break;
//						}
//
//						uint64_t mixing_freq = llabs(harmonic_freq_1LO - harmonic_freq_2LO);
//						if(mixing_freq > max_mixing_freq) {
//							// ADC filter will take care of frequencies this high
//							continue;
//						}
//						if(abs(Util::Alias(mixing_freq, HW::ADCSamplerate) - HW::IF2) <= actualBandwidth * 2) {
//							// this frequency point aliases into the final IF frequency, need to shift ADC samplerate
//							LOG_INFO("ADC alias with f=%lu%06luHz at point %lu (%lu%06luHz), 1.LO harmonic: %d, 2.LO harmonic: %d",
//														(uint32_t ) (mixing_freq / 1000000),
//														(uint32_t ) (mixing_freq % 1000000),i,  (uint32_t ) (freq / 1000000),
//														(uint32_t ) (freq % 1000000), (uint16_t) harmonic_1LO, (uint16_t) harmonic_2LO);
//							ADC_alias = true;
//							// select different 1.LO
//							LOFreq -= LO_shift_inc;
//							break;
//						}
//					}
//					if(ADC_alias) {
//						// already detected a problem, no need for further loop execution
//						break;
//					}
//				}
//			} while(ADC_alias && LOFreq >= freq + HW::IF1 - (max_LO_shift - LO_shift_inc));
//		}

		if(harmonic_mixing) {
			LOFreq /= LOHarmonic;
		}
		LO1.SetFrequency(LOFreq);
		uint64_t actualLO1 = LO1.GetActualFrequency();
		if(harmonic_mixing) {
			actualLO1 *= LOHarmonic;
		}
		uint32_t actualFirstIF = actualLO1 - actualSourceFreq;
		uint32_t actualFinalIF = actualFirstIF - last_LO2;
		uint32_t IFdeviation = abs(actualFinalIF - HW::IF2);
		bool needs_LO2_shift = false;
		if(IFdeviation > actualBandwidth / 2) {
			needs_LO2_shift = true;
		}
		if (s.suppressPeaks && needs_LO2_shift) {
			if (IFTableIndexCnt < IFTableNumEntries) {
				// still room in table
				needs_halt = true;
				IFTable[IFTableIndexCnt].pointCnt = i;
				if(IFTableIndexCnt < IFTableNumEntries - 1) {
					// Configure LO2 for the changed IF1. This is not necessary right now but it will generate
					// the correct clock settings
					last_LO2 = actualFirstIF - HW::IF2;
					LOG_INFO("Changing 2.LO to %lu at point %lu (%lu%06luHz) to reach correct 2.IF frequency (1.LO: %lu%06luHz, 1.IF: %lu%06luHz)",
							last_LO2, i, (uint32_t ) (freq / 1000000),
							(uint32_t ) (freq % 1000000), (uint32_t ) (actualLO1 / 1000000),
							(uint32_t ) (actualLO1 % 1000000), (uint32_t ) (actualFirstIF / 1000000),
							(uint32_t ) (actualFirstIF % 1000000));
				} else {
					// last entry in IF table, revert LO2 to default
					last_LO2 = HW::IF1 - HW::IF2;
				}
				Si5351.SetCLK(SiChannel::RefLO2, last_LO2,
						Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
				// store calculated clock configuration for later change
				Si5351.ReadRawCLKConfig(SiChannel::RefLO2, IFTable[IFTableIndexCnt].clkconfig);
				IFTableIndexCnt++;
				needs_LO2_shift = false;
			}
		}
		if(needs_LO2_shift) {
			// if shift is still needed either peak suppression is disabled or no more room in IFTable was available
			LOG_WARN(
					"PLL deviation of %luHz for measurement at %lu%06luHz, will cause a peak",
					IFdeviation, (uint32_t ) (freq / 1000000), (uint32_t ) (freq % 1000000));
		}

		// Check if we have an ADC alias problem at this frequency. The 1.LO (and its harmonics) will mix with
		// the 2.LO (and its harmonics) in the 2.mixer stage. The mixing product may alias to the final IF in
		// the ADCs. This would cause a peak at this frequency in the spectrum
//		bool ADC_alias = false;
//		static uint8_t max_harmonic = 3;
//		for (uint64_t harmonic_1LO = 1; harmonic_1LO <= max_harmonic;
//				harmonic_1LO++) {
//			for (uint64_t harmonic_2LO = 1; harmonic_2LO <= max_harmonic;
//					harmonic_2LO++) {
//				uint64_t harmonic_freq_1LO = harmonic_1LO * actualLO1;
//				uint64_t harmonic_freq_2LO = harmonic_2LO * last_LO2;
//
//				uint64_t mixing_freq = abs(harmonic_freq_1LO - harmonic_freq_2LO);
//				if(abs(Util::Alias(mixing_freq, HW::ADCSamplerate) - HW::IF2) <= actualBandwidth * 2) {
//					// this frequency point aliases into the final IF frequency, need to shift ADC samplerate
//					ADC_alias = true;
//					break;
//				}
//			}
//			if(ADC_alias) {
//				// already detected a problem, no need for further loop execution
//				break;
//			}
//		}
//
//		uint8_t required_prescaler = HW::ADCprescaler;
//		if(ADC_alias) {
//			required_prescaler = alternativePrescaler;
//		}
//
//		if(required_prescaler != last_ADC_prescaler) {
//			// needs to shift ADC prescaler at this point
//			if (ADCTableIndexCnt < ADCTableNumEntries) {
//				// still room in table
//				needs_halt = true;
//				ADCTable[ADCTableIndexCnt].pointCnt = i;
//				if(ADCTableIndexCnt < ADCTableNumEntries - 1) {
//					ADCTable[ADCTableIndexCnt].adcPrescaler = required_prescaler;
//				} else {
//					// last entry in ADC table, revert ADC prescaler to default
//					ADCTable[ADCTableIndexCnt].adcPrescaler = HW::ADCprescaler;
//				}
//				last_ADC_prescaler = ADCTable[ADCTableIndexCnt].adcPrescaler;
//				LOG_INFO("Changing ADC prescaler to %d at point %lu (%lu%06luHz) to prevent alias.",
//						ADCTable[ADCTableIndexCnt].adcPrescaler, i, (uint32_t ) (freq / 1000000),
//											(uint32_t ) (freq % 1000000));
//				ADCTableIndexCnt++;
//			}
//		}

		// halt on regular intervals to prevent USB buffer overflow
		if(!needs_halt) {
			pointsWithoutHalt++;
			if(pointsWithoutHalt > maxPointsBetweenHalts) {
				needs_halt = true;
			}
		}
		if(needs_halt) {
			pointsWithoutHalt = 0;
		}

		uint8_t attenuator = freq >= HW::BandSwitchFrequency ? fixedAttenuatorHighband : fixedAttenuatorLowband;
		if(!s.fixedPowerSetting) {
			// adapt power level throughout the sweep
			amplitude = HW::GetAmplitudeSettings(power, freq, true, false);
			if(freq >= HW::BandSwitchFrequency) {
				Source.SetPowerOutA(amplitude.highBandPower, true);
			}
			if(amplitude.unlevel) {
				HW::SetOutputUnlevel(true);
			}
			attenuator = amplitude.attenuator;
		}

		FPGA::WriteSweepConfig(i, lowband, Source.GetRegisters(),
				LO1.GetRegisters(), attenuator, freq, FPGA::SettlingTime::us20,
				FPGA::Samples::SPPRegister, needs_halt);
		last_lowband = lowband;
	}
	// revert clk configuration to previous value (might have been changed in sweep calculation)
	Si5351.SetCLK(SiChannel::RefLO2, HW::IF1 - HW::IF2, Si5351C::PLL::B, Si5351C::DriveStrength::mA2);
	Si5351.ResetPLL(Si5351C::PLL::B);
	if (s.f_start >= HW::BandSwitchFrequency) {
		// first point already uses the MAX2871 as the source and (probably) does not have the "halt sweep" bit
		// set. Give Si5351 some time to ramp up the 2.LO, otherwise the first samples might be invalid
		Delay::us(1300);
	}
	// Enable mixers/amplifier/PLLs
	FPGA::SetWindow(FPGA::Window::Hann);
	FPGA::Enable(FPGA::Periphery::Port1Mixer);
	FPGA::Enable(FPGA::Periphery::Port2Mixer);
	FPGA::Enable(FPGA::Periphery::RefMixer);
	FPGA::Enable(FPGA::Periphery::Amplifier);
	FPGA::Enable(FPGA::Periphery::SourceChip);
	FPGA::Enable(FPGA::Periphery::SourceRF);
	FPGA::Enable(FPGA::Periphery::LO1Chip);
	FPGA::Enable(FPGA::Periphery::LO1RF);
	FPGA::Enable(FPGA::Periphery::ExcitePort1, s.excitePort1);
	FPGA::Enable(FPGA::Periphery::ExcitePort2, s.excitePort2);
	FPGA::Enable(FPGA::Periphery::PortSwitch);
	pointCnt = 0;
	// starting port depends on whether port 1 is active in sweep
	excitingPort1 = s.excitePort1;
	// invalidate first invalid entry of IFTable, preventing switching of 2.LO in halted callback
	IFTable[IFTableIndexCnt].pointCnt = 0xFFFF;
	ADCTable[ADCTableIndexCnt].pointCnt = 0xFFFF;
	// reset table index
	IFTableIndexCnt = 0;
	ADCTableIndexCnt = 0;

	adcShifted = false;
	active = true;
	// Enable new data and sweep halt interrupt
	FPGA::EnableInterrupt(FPGA::Interrupt::NewData);
	FPGA::EnableInterrupt(FPGA::Interrupt::SweepHalted);
	// Start the sweep
	FPGA::StartSweep();
	return true;
}

static void PassOnData() {
	Protocol::PacketInfo info;
	info.type = Protocol::PacketType::Datapoint;
	info.datapoint = data;
	Communication::Send(info);
}

bool VNA::MeasurementDone(const FPGA::SamplingResult &result) {
	if(!active) {
		return false;
	}
	if(result.pointNum != pointCnt || !result.activePort != excitingPort1) {
		LOG_WARN("Indicated point does not match (%u != %u, %d != %d)", result.pointNum, pointCnt, result.activePort, !excitingPort1);
		FPGA::AbortSweep();
		return false;
	}
	// normal sweep mode
	auto port1_raw = std::complex<float>(result.P1I, result.P1Q);
	auto port2_raw = std::complex<float>(result.P2I, result.P2Q);
	auto ref = std::complex<float>(result.RefI, result.RefQ);
	auto port1 = port1_raw / ref;
	auto port2 = port2_raw / ref;
	data.pointNum = pointCnt;
	data.frequency = settings.f_start + (settings.f_stop - settings.f_start) * pointCnt / (settings.points - 1);
	data.cdbm = settings.cdbm_excitation_start + (settings.cdbm_excitation_stop - settings.cdbm_excitation_start) * pointCnt / (settings.points - 1);
	if(excitingPort1) {
		data.real_S11 = port1.real();
		data.imag_S11 = port1.imag();
		data.real_S21 = port2.real();
		data.imag_S21 = port2.imag();
	} else {
		data.real_S12 = port1.real();
		data.imag_S12 = port1.imag();
		data.real_S22 = port2.real();
		data.imag_S22 = port2.imag();
	}
	// figure out whether this sweep point is complete and which port gets excited next
	bool pointComplete = false;
	if(settings.excitePort1 == 1 && settings.excitePort2 == 1) {
		// point is complete when port 2 was active
		pointComplete = !excitingPort1;
		// next measurement will be from other port
		excitingPort1 = !excitingPort1;
	} else {
		// only one port active, point is complete after every measurement
		pointComplete = true;
	}
	if(pointComplete) {
		STM::DispatchToInterrupt(PassOnData);
		pointCnt++;
		if (pointCnt >= settings.points) {
			// reached end of sweep, start again
			pointCnt = 0;
			IFTableIndexCnt = 0;
			// request to trigger work function
			return true;
		}
	}
	return false;
}

void VNA::Work() {
	// end of sweep
	HW::Ref::update();
	// Compile info packet
	Protocol::PacketInfo packet;
	packet.type = Protocol::PacketType::DeviceInfo;
	HW::fillDeviceInfo(&packet.info, true);
	Communication::Send(packet);
	// do not reset unlevel flag here, as it is calculated only once at the setup of the sweep
	// Start next sweep
	FPGA::StartSweep();
}

void VNA::SweepHalted() {
	if(!active) {
		return;
	}
	LOG_DEBUG("Halted before point %d", pointCnt);
	// Check if IF table has entry at this point
	if (IFTableIndexCnt < IFTableNumEntries && IFTable[IFTableIndexCnt].pointCnt == pointCnt) {
		Si5351.WriteRawCLKConfig(SiChannel::Port1LO2, IFTable[IFTableIndexCnt].clkconfig);
		Si5351.WriteRawCLKConfig(SiChannel::Port2LO2, IFTable[IFTableIndexCnt].clkconfig);
		Si5351.WriteRawCLKConfig(SiChannel::RefLO2, IFTable[IFTableIndexCnt].clkconfig);
		Si5351.ResetPLL(Si5351C::PLL::B);
		IFTableIndexCnt++;
		// PLL reset causes the 2.LO to turn off briefly and then ramp on back, needs delay before next point
		Delay::us(1300);
	}
	uint64_t frequency = settings.f_start
			+ (settings.f_stop - settings.f_start) * pointCnt
					/ (settings.points - 1);
	int16_t power = settings.cdbm_excitation_start
			+ (settings.cdbm_excitation_stop - settings.cdbm_excitation_start)
					* pointCnt / (settings.points - 1);
	bool adcShiftRequired = false;
	if (frequency < HW::BandSwitchFrequency) {
		auto driveStrength = fixedPowerLowband;
		if(!settings.fixedPowerSetting) {
			auto amplitude = HW::GetAmplitudeSettings(power, frequency, true, false);
			// attenuator value has already been set in sweep setup
			driveStrength = amplitude.lowBandPower;
		}

		// need the Si5351 as Source
		Si5351.SetCLK(SiChannel::LowbandSource, frequency, Si5351C::PLL::B, driveStrength);
		if (pointCnt == 0) {
			// First point in sweep, enable CLK
			Si5351.Enable(SiChannel::LowbandSource);
			FPGA::Disable(FPGA::Periphery::SourceRF);
			Delay::us(1300);
		}

//		// At low frequencies the 1.LO feedthrough mixes with the 2.LO in the second mixer.
//		// Depending on the stimulus frequency, the resulting mixing product might alias to the 2.IF
//		// in the ADC which causes a spike. Check for this and shift the ADC sampling frequency if necessary
//		uint32_t LO_mixing = (HW::IF1 + frequency) - (HW::IF1 - HW::IF2);
//		if(abs(Util::Alias(LO_mixing, HW::ADCSamplerate) - HW::IF2) <= actualBandwidth * 2) {
//			// the image is in or near the IF bandwidth and would cause a peak
//			// Use a slightly different ADC samplerate
//			adcShiftRequired = true;
//		}
	} else if(!FPGA::IsEnabled(FPGA::Periphery::SourceRF)){
		// first sweep point in highband is also halted, disable lowband source
		Si5351.Disable(SiChannel::LowbandSource);
		FPGA::Enable(FPGA::Periphery::SourceRF);
	}

//	if(adcShiftRequired) {
//		FPGA::WriteRegister(FPGA::Reg::ADCPrescaler, alternativePrescaler);
//		FPGA::WriteRegister(FPGA::Reg::PhaseIncrement, alternativePhaseInc);
//		adcShifted = true;
//	} else if(adcShifted) {
//		// reset to default value
//		FPGA::WriteRegister(FPGA::Reg::ADCPrescaler, HW::ADCprescaler);
//		FPGA::WriteRegister(FPGA::Reg::PhaseIncrement, HW::DFTphaseInc);
//		adcShifted = false;
//	}
//	// Check if IF table has entry at this point
//	if (ADCTableIndexCnt < ADCTableNumEntries && ADCTable[ADCTableIndexCnt].pointCnt == pointCnt) {
//		FPGA::WriteRegister(FPGA::Reg::ADCPrescaler, ADCTable[ADCTableIndexCnt].adcPrescaler);
//		FPGA::WriteRegister(FPGA::Reg::PhaseIncrement, ADCTable[ADCTableIndexCnt].adcPrescaler * 10);
//		ADCTableIndexCnt++;
//	}

	if(usb_available_buffer() >= reservedUSBbuffer) {
		// enough space available, can resume immediately
		FPGA::ResumeHaltedSweep();
	} else {
		// USB buffer could potentially overflow before next halted point, wait until more space is available.
		// This function is called from a low level interrupt, need to dispatch to lower priority to allow USB
		// handling to continue
		STM::DispatchToInterrupt([](){
			uint32_t start = HAL_GetTick();
			while(usb_available_buffer() < reservedUSBbuffer) {
				if(HAL_GetTick() - start > 100) {
					// still no buffer space after some time, something more serious must have gone wrong
					// -> abort sweep and return to idle
					usb_clear_buffer();
					FPGA::AbortSweep();
					HW::SetIdle();
					return;
				}
			}
			FPGA::ResumeHaltedSweep();
		});
	}
}

void VNA::Stop() {
	active = false;
	FPGA::AbortSweep();
}