///////////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2019-2020 Edouard Griffiths, F4EXB <f4exb06@gmail.com>          //
//                                                                               //
// This program is free software; you can redistribute it and/or modify          //
// it under the terms of the GNU General Public License as published by          //
// the Free Software Foundation as version 3 of the License, or                  //
// (at your option) any later version.                                           //
//                                                                               //
// This program is distributed in the hope that it will be useful,               //
// but WITHOUT ANY WARRANTY; without even the implied warranty of                //
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the                  //
// GNU General Public License V3 for more details.                               //
//                                                                               //
// You should have received a copy of the GNU General Public License             //
// along with this program. If not, see <http://www.gnu.org/licenses/>.          //
///////////////////////////////////////////////////////////////////////////////////

#include <QTime>
#include <QDebug>
#include <QStringList>
#include <stdio.h>
#include <algorithm>
#include <cmath>
#include <vector>

#include "dsp/dsptypes.h"
#include "dsp/basebandsamplesink.h"
#include "dsp/dspengine.h"
#include "dsp/fftfactory.h"
#include "dsp/fftengine.h"
#include "util/db.h"

#include "meshtasticdemodmsg.h"
#include "meshtasticdemoddecoderlora.h"
#include "meshtasticdemodsink.h"

MeshtasticDemodSink::MeshtasticDemodSink() :
    m_decodeMsg(nullptr),
    m_decoderMsgQueue(nullptr),
    m_fftSequence(-1),
    m_downChirps(nullptr),
    m_upChirps(nullptr),
    m_spectrumLine(nullptr),
    m_headerLocked(false),
    m_expectedSymbols(0),
    m_waitHeaderFeedback(false),
    m_headerFeedbackWaitSteps(0U),
    m_loRaFrameId(0U),
    m_osFactor(MeshtasticDemodSettings::oversampling > 0 ? MeshtasticDemodSettings::oversampling : 1),
    m_osCenterPhase((MeshtasticDemodSettings::oversampling > 1 ? MeshtasticDemodSettings::oversampling / 2 : 0)),
    m_osCounter(0),
    m_loRaState(LoRaStateDetect),
    m_loRaSyncState(LoRaSyncNetId1),
    m_loRaSymbolCnt(1),
    m_loRaBinIdx(0),
    m_loRaKHat(0),
    m_loRaDownVal(0),
    m_loRaCFOInt(0),
    m_loRaNetIdOff(0),
    m_loRaAdditionalUpchirps(0),
    m_loRaUpSymbToUse(0),
    m_loRaRequiredUpchirps(0),
    m_loRaSymbolSpan(0),
    m_loRaFrameSymbolCount(0),
    m_loRaCFOFrac(0.0f),
    m_loRaSTOFrac(0.0f),
    m_loRaSFOHat(0.0f),
    m_loRaSFOCum(0.0f),
    m_loRaCFOSTOEstimated(false),
    m_loRaReceivedHeader(false),
    m_loRaOneSymbolOff(false),
    m_spectrumSink(nullptr),
    m_spectrumBuffer(nullptr)
{
    m_demodActive = false;
	m_bandwidth = MeshtasticDemodSettings::bandwidths[0];
	m_channelSampleRate = 96000;
	m_channelFrequencyOffset = 0;
    m_deviceCenterFrequency = 0;
	m_nco.setFreq(m_channelFrequencyOffset, m_channelSampleRate);
	m_interpolator.create(16, m_channelSampleRate, m_bandwidth / 1.9f);
    m_interpolatorDistance = (Real) m_channelSampleRate / (Real) m_bandwidth;
    m_sampleDistanceRemain = 0;
    const unsigned int ctorConfiguredPreamble = m_settings.m_preambleChirps > 0U
        ? m_settings.m_preambleChirps
        : m_minRequiredPreambleChirps;
    const unsigned int ctorTargetRequired = ctorConfiguredPreamble > 3U
        ? (ctorConfiguredPreamble - 3U)
        : m_minRequiredPreambleChirps;
    m_requiredPreambleChirps = std::max(
        m_minRequiredPreambleChirps,
        std::min(ctorTargetRequired, m_maxRequiredPreambleChirps)
    );
    m_fftInterpolation = m_loRaFFTInterpolation;

    initSF(m_settings.m_spreadFactor, m_settings.m_deBits);
}

MeshtasticDemodSink::~MeshtasticDemodSink()
{
    FFTFactory *fftFactory = DSPEngine::instance()->getFFTFactory();

    if (m_fftSequence >= 0)
    {
        fftFactory->releaseEngine(m_interpolatedFFTLength, false, m_fftSequence);
    }

    delete[] m_downChirps;
    delete[] m_upChirps;
    delete[] m_spectrumBuffer;
    delete[] m_spectrumLine;
}

void MeshtasticDemodSink::initSF(unsigned int sf, unsigned int deBits)
{
    if (m_downChirps) {
        delete[] m_downChirps;
    }
    if (m_upChirps) {
        delete[] m_upChirps;
    }
    if (m_spectrumBuffer) {
        delete[] m_spectrumBuffer;
    }
    if (m_spectrumLine) {
        delete[] m_spectrumLine;
    }

    FFTFactory *fftFactory = DSPEngine::instance()->getFFTFactory();

    if (m_fftSequence >= 0) {
        fftFactory->releaseEngine(m_interpolatedFFTLength, false, m_fftSequence);
    }

    m_nbSymbols = 1 << sf;
    m_nbSymbolsEff = 1 << (sf - deBits);
    m_fftLength = m_nbSymbols;
    m_interpolatedFFTLength = m_fftInterpolation*m_fftLength;
    m_fftSequence = fftFactory->getEngine(m_interpolatedFFTLength, false, &m_fft);
    m_downChirps = new Complex[2*m_nbSymbols]; // Each table is 2 chirps long to allow processing from arbitrary offsets.
    m_upChirps = new Complex[2*m_nbSymbols];
    m_spectrumBuffer = new Complex[m_nbSymbols];
    m_spectrumLine = new Complex[m_nbSymbols];
    std::fill(m_spectrumLine, m_spectrumLine+m_nbSymbols, Complex(std::polar(1e-6*SDR_RX_SCALED, 0.0)));
    m_loRaSymbolSpan = m_nbSymbols * m_osFactor;
    m_loRaRequiredUpchirps = m_requiredPreambleChirps;
    m_loRaUpSymbToUse = (m_loRaRequiredUpchirps > 0U) ? static_cast<int>(m_loRaRequiredUpchirps - 1U) : 0;
    m_loRaInDown.assign(m_nbSymbols, Complex{0.0f, 0.0f});
    m_loRaPreambleRaw.assign(m_nbSymbols * m_loRaRequiredUpchirps, Complex{0.0f, 0.0f});
    m_loRaPreambleRawUp.assign((m_settings.m_preambleChirps + 3U) * m_loRaSymbolSpan, Complex{0.0f, 0.0f});
    m_loRaPreambleUpchirps.assign(m_nbSymbols * m_loRaRequiredUpchirps, Complex{0.0f, 0.0f});
    m_loRaCFOFracCorrec.assign(m_nbSymbols, Complex{1.0f, 0.0f});
    m_loRaPayloadDownchirp.assign(m_nbSymbols, Complex{1.0f, 0.0f});
    m_loRaSymbCorr.assign(m_nbSymbols, Complex{0.0f, 0.0f});
    m_loRaNetIdSamp.assign((m_loRaSymbolSpan * 5U) / 2U + m_loRaSymbolSpan, Complex{0.0f, 0.0f});
    m_loRaAdditionalSymbolSamp.assign(m_loRaSymbolSpan * 2U, Complex{0.0f, 0.0f});
    m_loRaPreambleVals.assign(m_loRaRequiredUpchirps, 0);
    m_loRaNetIds.assign(2, 0);
    m_loRaSampleFifo.clear();
    m_loRaState = LoRaStateDetect;
    m_loRaSyncState = LoRaSyncNetId1;
    m_loRaSymbolCnt = 1;
    m_loRaBinIdx = 0;
    m_loRaKHat = 0;
    m_loRaAdditionalUpchirps = 0;
    m_loRaCFOSTOEstimated = false;
    m_loRaReceivedHeader = false;
    m_loRaFrameSymbolCount = 0;

    // Canonical gr-lora_sdr reference chirps (utilities::build_ref_chirps, id=0, os_factor=1).
    for (unsigned int i = 0; i < m_fftLength; i++)
    {
        const double n = static_cast<double>(i);
        const double N = static_cast<double>(m_nbSymbols);
        const double phase = 2.0 * M_PI * ((n * n) / (2.0 * N) - 0.5 * n);
        m_upChirps[i] = Complex(std::cos(phase), std::sin(phase));
        m_downChirps[i] = std::conj(m_upChirps[i]);
    }

    // Duplicate table to allow processing from arbitrary offsets
    std::copy(m_downChirps, m_downChirps+m_fftLength, m_downChirps+m_fftLength);
    std::copy(m_upChirps, m_upChirps+m_fftLength, m_upChirps+m_fftLength);
}

void MeshtasticDemodSink::feed(const SampleVector::const_iterator& begin, const SampleVector::const_iterator& end)
{
	Complex ci;

	for (SampleVector::const_iterator it = begin; it < end; ++it)
	{
		Complex c(it->real() / SDR_RX_SCALEF, it->imag() / SDR_RX_SCALEF);
		c *= m_nco.nextIQ();

		if (m_interpolator.decimate(&m_sampleDistanceRemain, c, &ci))
		{
            if (MeshtasticDemodSettings::m_codingScheme == MeshtasticDemodSettings::CodingLoRa)
            {
                processSampleLoRa(ci);
            }
            else if (m_osFactor <= 1U)
            {
                processSampleLoRa(ci);
            }
            else
            {
                if ((m_osCounter % m_osFactor) == m_osCenterPhase) {
                    processSampleLoRa(ci);
                }
                m_osCounter++;
            }
			m_sampleDistanceRemain += m_interpolatorDistance;
		}
	}
}

void MeshtasticDemodSink::reset()
{
    resetLoRaFrameSync();
    m_headerLocked = false;
    m_expectedSymbols = 0;
    m_waitHeaderFeedback = false;
    m_headerFeedbackWaitSteps = 0;
    m_osCounter = 0;
}

unsigned int MeshtasticDemodSink::argmax(
    const Complex *fftBins,
    unsigned int fftMult,
    unsigned int fftLength,
    double& magsqMax,
    double& magsqTotal,
    Complex *specBuffer,
    unsigned int specDecim)
{
    magsqMax = 0.0;
    magsqTotal = 0.0;
    unsigned int imax = 0;
    double magSum = 0.0;
    std::vector<double> spectrumBucketPowers;

    if (specBuffer) {
        spectrumBucketPowers.reserve((fftMult * fftLength) / std::max(1U, specDecim));
    }

    for (unsigned int i = 0; i < fftMult*fftLength; i++)
    {
        double magsq = std::norm(fftBins[i]);
        magsqTotal += magsq;

        if (magsq > magsqMax)
        {
            imax = i;
            magsqMax = magsq;
        }

        if (specBuffer)
        {
            magSum += magsq;

            if (i % specDecim == specDecim - 1)
            {
                spectrumBucketPowers.push_back(magSum);
                magSum = 0.0;
            }
        }
    }

    const double magsqAvgRaw = magsqTotal / static_cast<double>(fftMult * fftLength);
    magsqTotal = magsqAvgRaw;

    if (specBuffer && !spectrumBucketPowers.empty())
    {
        const double noisePerBucket = magsqAvgRaw * static_cast<double>(std::max(1U, specDecim));
        const double floorCut = noisePerBucket * 1.05; // suppress steady floor
        const double boost = 12.0;                     // emphasize peaks over residual floor

        for (size_t i = 0; i < spectrumBucketPowers.size(); ++i)
        {
            const double enhancedPower = std::max(0.0, spectrumBucketPowers[i] - floorCut) * boost;
            const double specAmp = std::sqrt(enhancedPower) * static_cast<double>(m_nbSymbols);
            specBuffer[i] = Complex(std::polar(specAmp, 0.0));
        }
    }

    return imax;
}

unsigned int MeshtasticDemodSink::evalSymbol(unsigned int rawSymbol, bool headerSymbol)
{
    unsigned int spread = m_fftInterpolation * (1U << m_settings.m_deBits);
    const unsigned int symbolBins = m_fftInterpolation * m_nbSymbols;

    if (symbolBins == 0U) {
        return rawSymbol;
    }

    // In gr-lora_sdr, explicit-header symbols are always reduced by 2 extra bits
    // (sf_app = sf-2), independently of payload LDRO selection.
    if (headerSymbol)
    {
        const int de = m_settings.m_deBits;
        if (de < 2) {
            spread <<= (2 - de);
        }
    }

    // Match gr-lora_sdr hard-decoding symbol mapping:
    //   s = mod(raw_bin - 1, 2^SF * os_factor) / (os_factor * 2^DE)
    const unsigned int shifted = (rawSymbol + symbolBins - 1U) % symbolBins;

    if (spread == 0U) {
        return shifted;
    }

    return shifted / spread;
}

void MeshtasticDemodSink::tryHeaderLock()
{
    if (!m_decodeMsg) {
        return;
    }

    const std::vector<unsigned short>& symbols = m_decodeMsg->getSymbols();

    if (symbols.size() < 8) {
        return;
    }

    const unsigned int sf = m_settings.m_spreadFactor;

    if (sf < 7) {
        return;
    }

    const unsigned int headerNbSymbolBits = sf - 2U;

    if (headerNbSymbolBits < 5) {
        return;
    }

    const unsigned int maxOffset = std::min<unsigned int>(2U, static_cast<unsigned int>(symbols.size()) - 8U);
    const unsigned int headerSymbolMod = 1U << headerNbSymbolBits;

    for (unsigned int offset = 0U; offset <= maxOffset; offset++)
    {
        const std::vector<unsigned short> baseHeaderSlice(symbols.begin() + offset, symbols.begin() + offset + 8U);

        for (int delta = -2; delta <= 2; delta++)
        {
            std::vector<unsigned short> headerSlice(baseHeaderSlice);

            if (delta != 0)
            {
                for (auto& sym : headerSlice) {
                    const int shifted = loRaMod(static_cast<int>(sym) + delta, static_cast<int>(headerSymbolMod));
                    sym = static_cast<unsigned short>(shifted);
                }
            }

            bool hasCRC = true;
            unsigned int nbParityBits = 1U;
            unsigned int packetLength = 0U;
            int headerParityStatus = (int) MeshtasticDemodSettings::ParityUndefined;
            bool headerCRCStatus = false;

            MeshtasticDemodDecoderLoRa::decodeHeader(
                headerSlice,
                headerNbSymbolBits,
                hasCRC,
                nbParityBits,
                packetLength,
                headerParityStatus,
                headerCRCStatus
            );

            if (!headerCRCStatus || packetLength == 0U || nbParityBits < 1U || nbParityBits > 4U) {
                continue;
            }

            const double symbolDurationMs = (double)(1U << sf) * 1000.0 / (double)m_bandwidth;
            const bool ldro = symbolDurationMs > 16.0;
            const unsigned int sfDenom = sf - (ldro ? 2U : 0U);

            // gr-lora_sdr formula: symb_numb = 8 + ceil(max(0, 2*pay_len - sf + 2 + 5 + has_crc*4) / (sf - 2*ldro)) * (4 + cr)
            const int numerator = 2 * (int)packetLength - (int)sf + 2 + 5 + (hasCRC ? 4 : 0);
            unsigned int payloadBlocks = 0;

            if (numerator > 0 && sfDenom > 0) {
                payloadBlocks = ((unsigned int)numerator + sfDenom - 1U) / sfDenom;
            }

            const unsigned int expectedSymbols = 8U + payloadBlocks * (4U + nbParityBits);

            if (expectedSymbols > m_settings.m_nbSymbolsMax) {
                continue;
            }

            if (offset > 0U)
            {
                m_decodeMsg->dropFront(offset);
                m_loRaFrameSymbolCount = m_loRaFrameSymbolCount > offset
                    ? (m_loRaFrameSymbolCount - offset)
                    : 0U;
            }

            m_expectedSymbols = expectedSymbols;
            m_headerLocked = true;

            // qDebug("[LOOPBACK][RX] header_realign frameId=%u offset=%u delta=%d", m_loRaFrameId, offset, delta);
            qDebug("MeshtasticDemodSink::tryHeaderLock: LOCKED len=%u CR=%u CRC=%s LDRO=%s expected=%u symbols offset=%u delta=%d",
                   packetLength, nbParityBits, hasCRC ? "on" : "off", ldro ? "on" : "off", m_expectedSymbols, offset, delta);
            return;
        }
    }

    if (symbols.size() >= 10U) {
        qDebug("MeshtasticDemodSink::tryHeaderLock: header invalid after offsets 0..%u deltas -2..2", maxOffset);
    }
}

bool MeshtasticDemodSink::sendLoRaHeaderProbe()
{
    if (!m_decodeMsg || !m_decoderMsgQueue) {
        return false;
    }

    const std::vector<unsigned short>& symbols = m_decodeMsg->getSymbols();

    if (symbols.size() < 8U) {
        return false;
    }

    std::vector<unsigned short> headerSymbols(symbols.begin(), symbols.begin() + 8);
    const unsigned int payloadNbSymbolBits = (m_settings.m_spreadFactor > m_settings.m_deBits)
        ? (m_settings.m_spreadFactor - m_settings.m_deBits)
        : 1U;
    const unsigned int headerNbSymbolBits = (static_cast<unsigned int>(m_settings.m_spreadFactor) > 2U)
        ? (m_settings.m_spreadFactor - 2U)
        : payloadNbSymbolBits;

    MeshtasticDemodMsg::MsgLoRaHeaderProbe *probe = MeshtasticDemodMsg::MsgLoRaHeaderProbe::create(
        m_loRaFrameId,
        headerSymbols,
        payloadNbSymbolBits,
        headerNbSymbolBits,
        m_settings.m_spreadFactor,
        static_cast<unsigned int>(std::max(1, m_bandwidth)),
        MeshtasticDemodSettings::m_hasHeader,
        MeshtasticDemodSettings::m_hasCRC
    );
    m_decoderMsgQueue->push(probe);

    return true;
}

void MeshtasticDemodSink::applyLoRaHeaderFeedback(
    uint32_t frameId,
    bool valid,
    bool hasCRC,
    unsigned int nbParityBits,
    unsigned int packetLength,
    bool ldro,
    unsigned int expectedSymbols,
    int headerParityStatus,
    bool headerCRCStatus)
{
    (void) hasCRC;
    (void) ldro;
    (void) headerParityStatus;

    if (m_loRaState != LoRaStateSFOCompensation)
    {
        return;
    }

    if (frameId != m_loRaFrameId) {
        return;
    }

    m_waitHeaderFeedback = false;
    m_headerFeedbackWaitSteps = 0;

    if (!valid || !headerCRCStatus || (packetLength == 0U) || (nbParityBits < 1U) || (nbParityBits > 4U))
    {
        qDebug("MeshtasticDemodSink::applyLoRaHeaderFeedback: invalid header -> reset frame");
        resetLoRaFrameSync();
        return;
    }

    if (expectedSymbols > m_settings.m_nbSymbolsMax)
    {
        qDebug("MeshtasticDemodSink::applyLoRaHeaderFeedback: expected %u > max %u, fallback to EOM",
            expectedSymbols, m_settings.m_nbSymbolsMax);
        return;
    }

    m_expectedSymbols = expectedSymbols;
    m_headerLocked = true;
    m_loRaReceivedHeader = true;
}

int MeshtasticDemodSink::loRaMod(int a, int b) const
{
    if (b <= 0) {
        return 0;
    }

    return (a % b + b) % b;
}

int MeshtasticDemodSink::loRaRound(float number) const
{
    return (number > 0.0f) ? static_cast<int>(number + 0.5f) : static_cast<int>(std::ceil(number - 0.5f));
}

void MeshtasticDemodSink::resetLoRaFrameSync()
{
    if (m_decodeMsg && (m_loRaState != LoRaStateDetect))
    {
        delete m_decodeMsg;
        m_decodeMsg = nullptr;
    }

    m_loRaState = LoRaStateDetect;
    m_loRaSyncState = LoRaSyncNetId1;
    m_loRaSampleFifo.clear();
    m_loRaSymbolCnt = 1;
    m_loRaBinIdx = 0;
    m_loRaKHat = 0;
    m_loRaDownVal = 0;
    m_loRaCFOInt = 0;
    m_loRaNetIdOff = 0;
    m_loRaAdditionalUpchirps = 0;
    m_loRaCFOFrac = 0.0f;
    m_loRaSTOFrac = 0.0f;
    m_loRaSFOHat = 0.0f;
    m_loRaSFOCum = 0.0f;
    m_loRaCFOSTOEstimated = false;
    m_loRaReceivedHeader = false;
    m_loRaOneSymbolOff = false;
    m_loRaFrameSymbolCount = 0;
    m_demodActive = false;
    m_headerLocked = false;
    m_expectedSymbols = 0;
    m_waitHeaderFeedback = false;
    m_headerFeedbackWaitSteps = 0;

    if (!m_loRaPreambleVals.empty()) {
        std::fill(m_loRaPreambleVals.begin(), m_loRaPreambleVals.end(), 0);
    }

    if (!m_loRaCFOFracCorrec.empty()) {
        std::fill(m_loRaCFOFracCorrec.begin(), m_loRaCFOFracCorrec.end(), Complex{1.0f, 0.0f});
    }
}

void MeshtasticDemodSink::clearSpectrumHistoryForNewFrame()
{
    if (!m_spectrumSink || !m_spectrumLine) {
        return;
    }

    // Insert a short floor separator between frames to make packet boundaries
    // visible even when consecutive packets arrive with a short idle gap.
    static constexpr unsigned int kSeparatorLines = 16U;
    for (unsigned int i = 0; i < kSeparatorLines; ++i) {
        m_spectrumSink->feed(m_spectrumLine, m_nbSymbols);
    }
}

unsigned int MeshtasticDemodSink::getLoRaSymbolVal(
    const Complex *samples,
    const Complex *refChirp,
    std::vector<float> *symbolMagnitudes,
    bool publishSpectrum
)
{
    for (unsigned int i = 0; i < m_nbSymbols; i++) {
        m_fft->in()[i] = samples[i] * refChirp[i];
    }

    // Canonical gr-lora_sdr demod uses a rectangular symbol window for
    // frame_sync/fft_demod symbol decisions. Do not apply user-selected FFT
    // windows in LoRa mode, otherwise header symbols drift and CRC checks fail.

    if (m_interpolatedFFTLength > m_fftLength) {
        std::fill(m_fft->in() + m_fftLength, m_fft->in() + m_interpolatedFFTLength, Complex{0.0f, 0.0f});
    }

    m_fft->transform();

    if (symbolMagnitudes)
    {
        symbolMagnitudes->assign(m_nbSymbols, 0.0f);

        for (unsigned int i = 0; i < m_nbSymbols; i++) {
            (*symbolMagnitudes)[i] = static_cast<float>(std::norm(m_fft->out()[i]));
        }
    }

    double magsq = 0.0;
    double magsqTotal = 0.0;
    const bool canCaptureSpectrum = (m_spectrumBuffer != nullptr);
    const bool publishSpectrumNow = publishSpectrum && (m_spectrumSink != nullptr) && canCaptureSpectrum;
    const unsigned int imax = argmax(
        m_fft->out(),
        m_fftInterpolation,
        m_fftLength,
        magsq,
        magsqTotal,
        canCaptureSpectrum ? m_spectrumBuffer : nullptr,
        m_fftInterpolation
    );

    if (publishSpectrumNow) {
        m_spectrumSink->feed(m_spectrumBuffer, m_nbSymbols);
    }

    return imax / m_fftInterpolation;
}

float MeshtasticDemodSink::estimateLoRaCFOFracBernier(const Complex *samples)
{
    if (m_loRaUpSymbToUse <= 1) {
        return 0.0f;
    }

    std::vector<int> k0(m_loRaUpSymbToUse, 0);
    std::vector<double> k0Mag(m_loRaUpSymbToUse, 0.0);
    std::vector<Complex> fftVal(m_loRaUpSymbToUse * m_nbSymbols);
    std::vector<Complex> dechirped(m_nbSymbols);

    for (int i = 0; i < m_loRaUpSymbToUse; i++)
    {
        const Complex *sym = samples + i * m_nbSymbols;

        for (unsigned int j = 0; j < m_nbSymbols; j++) {
            dechirped[j] = sym[j] * m_downChirps[j];
            m_fft->in()[j] = dechirped[j];
        }

        if (m_interpolatedFFTLength > m_fftLength) {
            std::fill(m_fft->in() + m_fftLength, m_fft->in() + m_interpolatedFFTLength, Complex{0.0f, 0.0f});
        }

        m_fft->transform();

        double magsq = 0.0;
        double magsqTotal = 0.0;
        const unsigned int imax = argmax(
            m_fft->out(),
            m_fftInterpolation,
            m_fftLength,
            magsq,
            magsqTotal,
            nullptr,
            m_fftInterpolation
        ) / m_fftInterpolation;
        k0[i] = static_cast<int>(imax);
        k0Mag[i] = magsq;

        for (unsigned int j = 0; j < m_nbSymbols; j++) {
            fftVal[j + i * m_nbSymbols] = m_fft->out()[j];
        }
    }

    const int idxMax = k0[std::distance(k0Mag.begin(), std::max_element(k0Mag.begin(), k0Mag.end()))];
    Complex fourCum(0.0f, 0.0f);

    for (int i = 0; i < m_loRaUpSymbToUse - 1; i++) {
        fourCum += fftVal[idxMax + m_nbSymbols * i] * std::conj(fftVal[idxMax + m_nbSymbols * (i + 1)]);
    }

    const float cfoFrac = -std::arg(fourCum) / (2.0f * static_cast<float>(M_PI));
    const unsigned int corrCount = static_cast<unsigned int>(m_loRaUpSymbToUse) * m_nbSymbols;

    for (unsigned int n = 0; n < corrCount && n < m_loRaPreambleUpchirps.size(); n++)
    {
        const float phase = -2.0f * static_cast<float>(M_PI) * cfoFrac * static_cast<float>(n) / static_cast<float>(m_nbSymbols);
        m_loRaPreambleUpchirps[n] = samples[n] * Complex(std::cos(phase), std::sin(phase));
    }

    return cfoFrac;
}

float MeshtasticDemodSink::estimateLoRaSTOFrac()
{
    if (m_loRaUpSymbToUse <= 0) {
        return 0.0f;
    }

    FFTFactory *fftFactory = DSPEngine::instance()->getFFTFactory();
    FFTEngine *fft2N = nullptr;
    const unsigned int fft2NLen = 2U * m_nbSymbols;
    const int fft2NSeq = fftFactory->getEngine(fft2NLen, false, &fft2N);
    std::vector<double> fftMagSq(fft2NLen, 0.0);
    std::vector<Complex> dechirped(m_nbSymbols);

    for (int i = 0; i < m_loRaUpSymbToUse; i++)
    {
        const Complex *sym = m_loRaPreambleUpchirps.data() + i * m_nbSymbols;

        for (unsigned int j = 0; j < m_nbSymbols; j++) {
            dechirped[j] = sym[j] * m_downChirps[j];
            fft2N->in()[j] = dechirped[j];
        }

        std::fill(fft2N->in() + m_nbSymbols, fft2N->in() + fft2NLen, Complex{0.0f, 0.0f});
        fft2N->transform();

        for (unsigned int j = 0; j < fft2NLen; j++) {
            fftMagSq[j] += std::norm(fft2N->out()[j]);
        }
    }

    fftFactory->releaseEngine(fft2NLen, false, fft2NSeq);

    const int k0 = static_cast<int>(std::distance(fftMagSq.begin(), std::max_element(fftMagSq.begin(), fftMagSq.end())));
    const double Y_1 = fftMagSq[loRaMod(k0 - 1, static_cast<int>(fft2NLen))];
    const double Y0 = fftMagSq[k0];
    const double Y1 = fftMagSq[loRaMod(k0 + 1, static_cast<int>(fft2NLen))];
    const double u = 64.0 * m_nbSymbols / 406.5506497;
    const double v = u * 2.4674;
    const double wa = (Y1 - Y_1) / (u * (Y1 + Y_1) + v * Y0 + 1e-12);
    const double ka = wa * m_nbSymbols / M_PI;
    const double kres = std::fmod((k0 + ka) / 2.0, 1.0);

    return static_cast<float>(kres - (kres > 0.5 ? 1.0 : 0.0));
}

void MeshtasticDemodSink::buildLoRaPayloadDownchirp()
{
    const int N = static_cast<int>(m_nbSymbols);
    const int id = loRaMod(m_loRaCFOInt, N);

    for (int n = 0; n < N; n++)
    {
        const int nFold = N - id;
        const double nD = static_cast<double>(n);
        const double ND = static_cast<double>(N);
        double phase;

        if (n < nFold) {
            phase = 2.0 * M_PI * ((nD * nD) / (2.0 * ND) + (static_cast<double>(id) / ND - 0.5) * nD);
        } else {
            phase = 2.0 * M_PI * ((nD * nD) / (2.0 * ND) + (static_cast<double>(id) / ND - 1.5) * nD);
        }

        const Complex up(std::cos(phase), std::sin(phase));
        Complex ref = m_settings.m_invertRamps ? up : std::conj(up);
        const float cfoPhase = -2.0f * static_cast<float>(M_PI) * m_loRaCFOFrac * static_cast<float>(n) / static_cast<float>(N);
        ref *= Complex(std::cos(cfoPhase), std::sin(cfoPhase));
        m_loRaPayloadDownchirp[n] = ref;
    }
}

void MeshtasticDemodSink::finalizeLoRaFrame()
{
    if (!m_decodeMsg) {
        resetLoRaFrameSync();
        return;
    }

    qDebug(
        "MeshtasticDemodSink::finalizeLoRaFrame: frameId=%u symbols=%u headerLocked=%d expected=%u",
        m_loRaFrameId,
        m_loRaFrameSymbolCount,
        m_headerLocked ? 1 : 0,
        m_expectedSymbols
    );

    m_decodeMsg->setSignalDb(CalcDb::dbPower(m_magsqOnAvg.asDouble() / (1 << m_settings.m_spreadFactor)));
    m_decodeMsg->setNoiseDb(CalcDb::dbPower(m_magsqOffAvg.asDouble() / (1 << m_settings.m_spreadFactor)));

    if (m_decoderMsgQueue && m_settings.m_decodeActive) {
        m_decoderMsgQueue->push(m_decodeMsg);
    } else {
        delete m_decodeMsg;
    }

    m_decodeMsg = nullptr;
    resetLoRaFrameSync();
}

void MeshtasticDemodSink::processSampleLoRa(const Complex& ci)
{
    m_loRaSampleFifo.push_back(ci);

    while (true)
    {
        const unsigned int needed = (m_loRaState == LoRaStateSync) ? (3U * m_loRaSymbolSpan) : m_loRaSymbolSpan;

        if (m_loRaSampleFifo.size() < needed) {
            return;
        }

        int consumed = processLoRaFrameSyncStep();

        if (consumed <= 0) {
            consumed = 1;
        }

        consumed = std::min(consumed, static_cast<int>(m_loRaSampleFifo.size()));

        for (int i = 0; i < consumed; i++) {
            m_loRaSampleFifo.pop_front();
        }
    }
}

int MeshtasticDemodSink::processLoRaFrameSyncStep()
{
    const int stoShift = loRaRound(m_loRaSTOFrac * static_cast<float>(m_osFactor));

    for (unsigned int ii = 0; ii < m_nbSymbols; ii++)
    {
        int idx = static_cast<int>(m_osCenterPhase + m_osFactor * ii) - stoShift;
        idx = std::max(0, std::min(idx, static_cast<int>(m_loRaSymbolSpan) - 1));
        m_loRaInDown[ii] = m_loRaSampleFifo[static_cast<size_t>(idx)];
    }

    if (m_loRaState == LoRaStateDetect)
    {
        const Complex *detectRef = m_settings.m_invertRamps ? m_upChirps : m_downChirps;
        // Keep last decoded packet visible until next frame starts.
        const int binNew = static_cast<int>(getLoRaSymbolVal(m_loRaInDown.data(), detectRef, nullptr, false));
        const int detectDelta = std::abs(loRaMod(std::abs(binNew - m_loRaBinIdx) + 1, static_cast<int>(m_nbSymbols)) - 1);
        const bool isConsecutive = (detectDelta <= 1);
        double symbolPower = 0.0;

        for (const Complex &s : m_loRaInDown) {
            symbolPower += std::norm(s);
        }

        symbolPower /= std::max(1U, m_nbSymbols);
        m_magsqTotalAvg(symbolPower);

        if (isConsecutive)
        {
            if (m_loRaSymbolCnt == 1 && !m_loRaPreambleVals.empty()) {
                m_loRaPreambleVals[0] = m_loRaBinIdx;
            }

            if ((m_loRaSymbolCnt >= 0) && (m_loRaSymbolCnt < static_cast<int>(m_loRaPreambleVals.size()))) {
                m_loRaPreambleVals[m_loRaSymbolCnt] = binNew;
            }

            const size_t sOfs = static_cast<size_t>(m_loRaSymbolCnt) * m_nbSymbols;
            if (sOfs + m_nbSymbols <= m_loRaPreambleRaw.size()) {
                std::copy_n(m_loRaInDown.begin(), m_nbSymbols, m_loRaPreambleRaw.begin() + sOfs);
            }

            const size_t upOfs = static_cast<size_t>(m_loRaSymbolCnt) * m_loRaSymbolSpan;
            if (upOfs + m_loRaSymbolSpan <= m_loRaPreambleRawUp.size()) {
                std::copy_n(m_loRaSampleFifo.begin(), m_loRaSymbolSpan, m_loRaPreambleRawUp.begin() + upOfs);
            }

            m_loRaSymbolCnt++;
        }
        else
        {
            m_magsqOffAvg(symbolPower);

            if (m_loRaPreambleRaw.size() >= m_nbSymbols) {
                std::copy_n(m_loRaInDown.begin(), m_nbSymbols, m_loRaPreambleRaw.begin());
            }

            if (m_loRaPreambleRawUp.size() >= m_loRaSymbolSpan) {
                std::copy_n(m_loRaSampleFifo.begin(), m_loRaSymbolSpan, m_loRaPreambleRawUp.begin());
            }

            m_loRaSymbolCnt = 1;
        }

        m_loRaBinIdx = binNew;

        if ((m_loRaSymbolCnt >= static_cast<int>(m_loRaRequiredUpchirps))
            && !m_loRaPreambleVals.empty())
        {
            m_loRaAdditionalUpchirps = 0;
            m_loRaState = LoRaStateSync;
            m_loRaSyncState = LoRaSyncNetId1;
            m_loRaSymbolCnt = 0;
            m_loRaCFOSTOEstimated = false;

            std::vector<unsigned int> hist(m_nbSymbols, 0U);
            unsigned int bestBin = 0U;
            unsigned int bestCount = 0U;

            for (int v : m_loRaPreambleVals)
            {
                const unsigned int b = static_cast<unsigned int>(loRaMod(v, static_cast<int>(m_nbSymbols)));
                const unsigned int c = ++hist[b];

                if (c > bestCount) {
                    bestCount = c;
                    bestBin = b;
                }
            }

            m_loRaKHat = static_cast<int>(bestBin);
            const int netStart = static_cast<int>(0.75f * static_cast<float>(m_loRaSymbolSpan)) - m_loRaKHat * static_cast<int>(m_osFactor);

            for (unsigned int i = 0; i < m_loRaSymbolSpan / 4U; i++)
            {
                const int src = std::max(0, std::min(netStart + static_cast<int>(i), static_cast<int>(m_loRaSampleFifo.size()) - 1));
                if (i < m_loRaNetIdSamp.size()) {
                    m_loRaNetIdSamp[i] = m_loRaSampleFifo[static_cast<size_t>(src)];
                }
            }

            return static_cast<int>(m_osFactor * (m_nbSymbols - bestBin));
        }

        return static_cast<int>(m_loRaSymbolSpan);
    }

    if (m_loRaState == LoRaStateSync)
    {
        if (!m_loRaCFOSTOEstimated)
        {
            const int cfoStart = std::max(0, static_cast<int>(m_nbSymbols) - m_loRaKHat);

            if (cfoStart < static_cast<int>(m_loRaPreambleRaw.size())) {
                m_loRaCFOFrac = estimateLoRaCFOFracBernier(m_loRaPreambleRaw.data() + cfoStart);
            } else {
                m_loRaCFOFrac = 0.0f;
            }

            m_loRaSTOFrac = estimateLoRaSTOFrac();

            for (unsigned int n = 0; n < m_nbSymbols; n++)
            {
                const float phase = -2.0f * static_cast<float>(M_PI) * m_loRaCFOFrac * static_cast<float>(n) / static_cast<float>(m_nbSymbols);
                m_loRaCFOFracCorrec[n] = Complex(std::cos(phase), std::sin(phase));
            }

            m_loRaCFOSTOEstimated = true;
        }

        for (unsigned int i = 0; i < m_nbSymbols; i++) {
            m_loRaSymbCorr[i] = m_loRaInDown[i] * m_loRaCFOFracCorrec[i];
        }

        const Complex *syncRef = m_settings.m_invertRamps ? m_upChirps : m_downChirps;
        const int binIdx = static_cast<int>(getLoRaSymbolVal(m_loRaSymbCorr.data(), syncRef, nullptr, true));

        switch (m_loRaSyncState)
        {
        case LoRaSyncNetId1:
            if ((binIdx == 0) || (binIdx == 1) || (binIdx == static_cast<int>(m_nbSymbols) - 1))
            {
                const size_t dstOfs = static_cast<size_t>(m_loRaRequiredUpchirps + m_loRaAdditionalUpchirps) * m_loRaSymbolSpan;

                if (dstOfs + m_loRaSymbolSpan <= m_loRaPreambleRawUp.size()) {
                    std::copy_n(m_loRaSampleFifo.begin(), m_loRaSymbolSpan, m_loRaPreambleRawUp.begin() + dstOfs);
                }

                m_loRaAdditionalUpchirps = std::min(m_loRaAdditionalUpchirps + 1, 3);
            }
            else
            {
                m_loRaSyncState = LoRaSyncNetId2;
                m_loRaNetIds[0] = binIdx;
            }
            break;
        case LoRaSyncNetId2:
            m_loRaSyncState = LoRaSyncDownchirp1;
            m_loRaNetIds[1] = binIdx;
            break;
        case LoRaSyncDownchirp1:
            m_loRaSyncState = LoRaSyncDownchirp2;
            break;
        case LoRaSyncDownchirp2:
            m_loRaDownVal = static_cast<int>(getLoRaSymbolVal(m_loRaSymbCorr.data(), m_settings.m_invertRamps ? m_downChirps : m_upChirps, nullptr, true));
            if (m_loRaAdditionalSymbolSamp.size() >= m_loRaSymbolSpan) {
                std::copy_n(m_loRaSampleFifo.begin(), m_loRaSymbolSpan, m_loRaAdditionalSymbolSamp.begin());
            }
            m_loRaSyncState = LoRaSyncQuarterDown;
            break;
        case LoRaSyncQuarterDown:
        default:
            if (m_loRaAdditionalSymbolSamp.size() >= 2U * m_loRaSymbolSpan) {
                std::copy_n(m_loRaSampleFifo.begin(), m_loRaSymbolSpan, m_loRaAdditionalSymbolSamp.begin() + m_loRaSymbolSpan);
            }

            if (static_cast<unsigned int>(m_loRaDownVal) < m_nbSymbols / 2U) {
                m_loRaCFOInt = static_cast<int>(std::floor(m_loRaDownVal / 2.0));
            } else {
                m_loRaCFOInt = static_cast<int>(std::floor((m_loRaDownVal - static_cast<int>(m_nbSymbols)) / 2.0));
            }

            // Preserve state-machine net ID bin indices for sync word extraction.
            // The corrLen-based refinement overwrites m_loRaNetIds but may produce
            // incorrect results when m_loRaNetIdSamp is not fully populated.
            const int netIdBin0 = m_loRaNetIds[0];
            const int netIdBin1 = m_loRaNetIds[1];

            const unsigned int upSymCount = std::min<unsigned int>(
                static_cast<unsigned int>(std::max(0, m_loRaUpSymbToUse)),
                static_cast<unsigned int>(m_loRaPreambleUpchirps.size() / std::max(1U, m_nbSymbols))
            );
            const unsigned int corrLen = upSymCount * m_nbSymbols;

            if (corrLen > 0U)
            {
                const int cfoIntMod = loRaMod(m_loRaCFOInt, static_cast<int>(m_nbSymbols));
                std::rotate(
                    m_loRaPreambleUpchirps.begin(),
                    m_loRaPreambleUpchirps.begin() + cfoIntMod,
                    m_loRaPreambleUpchirps.begin() + corrLen
                );

                std::vector<Complex> cfoIntCorrec(corrLen, Complex{1.0f, 0.0f});
                for (unsigned int n = 0; n < corrLen; n++)
                {
                    const float phase = -2.0f * static_cast<float>(M_PI)
                        * static_cast<float>(m_loRaCFOInt)
                        * static_cast<float>(n)
                        / static_cast<float>(m_nbSymbols);
                    cfoIntCorrec[n] = Complex(std::cos(phase), std::sin(phase));
                }

                for (unsigned int n = 0; n < corrLen; n++) {
                    m_loRaPreambleUpchirps[n] *= cfoIntCorrec[n];
                }

                if (m_deviceCenterFrequency > 0) {
                    m_loRaSFOHat =
                        (static_cast<float>(m_loRaCFOInt) + m_loRaCFOFrac)
                        * static_cast<float>(m_bandwidth)
                        / static_cast<float>(m_deviceCenterFrequency);
                } else {
                    m_loRaSFOHat = 0.0f;
                }

                std::vector<Complex> sfoCorrec(corrLen, Complex{1.0f, 0.0f});
                const double clkOff = static_cast<double>(m_loRaSFOHat) / static_cast<double>(m_nbSymbols);
                const double fs = static_cast<double>(m_bandwidth);
                const double fsP = fs * (1.0 - clkOff);
                const int N = static_cast<int>(m_nbSymbols);

                for (unsigned int n = 0; n < corrLen; n++)
                {
                    const double nMod = static_cast<double>(loRaMod(static_cast<int>(n), N));
                    const double nFloor = std::floor(static_cast<double>(n) / static_cast<double>(N));
                    const double q1 = (nMod * nMod) / (2.0 * static_cast<double>(N))
                        * ((m_bandwidth / fsP) * (m_bandwidth / fsP) - (m_bandwidth / fs) * (m_bandwidth / fs));
                    const double q2 = (nFloor * ((m_bandwidth / fsP) * (m_bandwidth / fsP) - (m_bandwidth / fsP))
                        + m_bandwidth / 2.0 * (1.0 / fs - 1.0 / fsP)) * nMod;
                    const double phase = -2.0 * M_PI * (q1 + q2);
                    sfoCorrec[n] = Complex(std::cos(phase), std::sin(phase));
                }

                for (unsigned int n = 0; n < corrLen; n++) {
                    m_loRaPreambleUpchirps[n] *= sfoCorrec[n];
                }

                const float tmpSto = estimateLoRaSTOFrac();
                const float diffSto = m_loRaSTOFrac - tmpSto;

                if (std::abs(diffSto) <= (static_cast<float>(m_osFactor) - 1.0f) / static_cast<float>(m_osFactor)) {
                    m_loRaSTOFrac = tmpSto;
                }

                std::vector<Complex> netIdsDec(2U * m_nbSymbols, Complex{0.0f, 0.0f});
                const int startOff = static_cast<int>(m_osFactor / 2U)
                    - loRaRound(m_loRaSTOFrac * static_cast<float>(m_osFactor))
                    + static_cast<int>(m_osFactor)
                        * (static_cast<int>(0.25f * static_cast<float>(m_nbSymbols)) + m_loRaCFOInt);

                for (unsigned int i = 0; i < 2U * m_nbSymbols; i++)
                {
                    const int idx = std::max(
                        0,
                        std::min(startOff + static_cast<int>(i * m_osFactor), static_cast<int>(m_loRaNetIdSamp.size()) - 1)
                    );
                    netIdsDec[i] = m_loRaNetIdSamp[static_cast<size_t>(idx)];
                }

                for (unsigned int i = 0; i < 2U * m_nbSymbols; i++)
                {
                    netIdsDec[i] *= cfoIntCorrec[i % std::max(1U, corrLen)];

                    const float phase = -2.0f * static_cast<float>(M_PI)
                        * m_loRaCFOFrac
                        * static_cast<float>(i % m_nbSymbols)
                        / static_cast<float>(m_nbSymbols);
                    netIdsDec[i] *= Complex(std::cos(phase), std::sin(phase));
                }

                const int netid1 = static_cast<int>(getLoRaSymbolVal(netIdsDec.data(), m_settings.m_invertRamps ? m_upChirps : m_downChirps));
                const int netid2 = static_cast<int>(getLoRaSymbolVal(netIdsDec.data() + m_nbSymbols, m_settings.m_invertRamps ? m_upChirps : m_downChirps));
                m_loRaNetIds[0] = netid1;
                m_loRaNetIds[1] = netid2;
                m_loRaNetIdOff = netid1;
            }
            else
            {
                if (m_deviceCenterFrequency > 0) {
                    m_loRaSFOHat =
                        (static_cast<float>(m_loRaCFOInt) + m_loRaCFOFrac)
                        * static_cast<float>(m_bandwidth)
                        / static_cast<float>(m_deviceCenterFrequency);
                } else {
                    m_loRaSFOHat = 0.0f;
                }
            }

            buildLoRaPayloadDownchirp();
            m_loRaFrameId++;
            m_decodeMsg = MeshtasticDemodMsg::MsgDecodeSymbols::create();
            m_decodeMsg->setFrameId(m_loRaFrameId);
            {
                // LoRa sync word is encoded across two net-ID chirps.
                // First chirp (index 0) carries the high nibble, second (index 1) the low nibble.
                // Each nibble N is mapped to bin N*8. Formula: syncWord = low + 16*high.
                // Use the coarse state-machine bin values (netIdBin0/1) which are reliably
                // set from the FFT in LoRaSyncNetId1/2 states, not the refined values which
                // require m_loRaNetIdSamp to be fully populated.
                const unsigned int hiNibble = static_cast<unsigned int>(std::round(static_cast<double>(netIdBin0) / 8.0)) & 0xFU;
                const unsigned int loNibble = static_cast<unsigned int>(std::round(static_cast<double>(netIdBin1) / 8.0)) & 0xFU;
                m_decodeMsg->setSyncWord(loNibble + 16U * hiNibble);
            }
            clearSpectrumHistoryForNewFrame();
            m_loRaFrameSymbolCount = 0U;
            m_magsqMax = 0.0;
            m_headerLocked = false;
            m_expectedSymbols = 0;
            m_waitHeaderFeedback = false;
            m_headerFeedbackWaitSteps = 0U;
            m_demodActive = true;
            m_loRaSTOFrac += m_loRaSFOHat * 4.25f;
            if (std::abs(m_loRaSTOFrac) > 0.5f) {
                m_loRaSTOFrac += (m_loRaSTOFrac > 0.0f) ? -1.0f : 1.0f;
            }
            const float stoQuant = static_cast<float>(loRaRound(m_loRaSTOFrac * static_cast<float>(m_osFactor)));
            m_loRaSFOCum = ((m_loRaSTOFrac * static_cast<float>(m_osFactor)) - stoQuant) / static_cast<float>(m_osFactor);
            m_loRaState = LoRaStateSFOCompensation;
            m_loRaSyncState = LoRaSyncNetId1;
            return std::max(1, static_cast<int>(m_loRaSymbolSpan / 4U + static_cast<int>(m_osFactor) * m_loRaCFOInt));
        }

        return static_cast<int>(m_loRaSymbolSpan);
    }

    if (!m_decodeMsg)
    {
        resetLoRaFrameSync();
        return static_cast<int>(m_loRaSymbolSpan);
    }

    std::vector<float> symbolMags;
    const unsigned int rawSymbol = getLoRaSymbolVal(m_loRaInDown.data(), m_loRaPayloadDownchirp.data(), &symbolMags, true);
    const bool headerSymbol = m_loRaFrameSymbolCount < 8U;
    const unsigned short symbol = evalSymbol(rawSymbol, headerSymbol) % m_nbSymbolsEff;
    m_decodeMsg->pushBackSymbol(symbol);
    m_decodeMsg->pushBackMagnitudes(symbolMags);

    if (m_spectrumBuffer)
    {
        std::vector<float> spectrumLine;
        spectrumLine.reserve(m_nbSymbols);

        for (unsigned int i = 0; i < m_nbSymbols; ++i) {
            spectrumLine.push_back(static_cast<float>(std::norm(m_spectrumBuffer[i])));
        }

        m_decodeMsg->pushBackDechirpedSpectrumLine(spectrumLine);
    }

    double magsq = 0.0;
    for (const Complex &s : m_loRaInDown) {
        magsq += std::norm(s);
    }
    magsq /= std::max(1U, m_nbSymbols);

    if (magsq > m_magsqMax) {
        m_magsqMax = magsq;
    }

    m_magsqTotalAvg(magsq);
    m_magsqOnAvg(magsq);
    m_loRaFrameSymbolCount++;

    if (!m_headerLocked
        && (m_loRaFrameSymbolCount >= 8U))
    {
        tryHeaderLock();
    }

    if (m_headerLocked)
    {
        if (m_loRaFrameSymbolCount >= m_expectedSymbols) {
            finalizeLoRaFrame();
        }
    }
    else if (m_loRaFrameSymbolCount >= m_settings.m_nbSymbolsMax)
    {
        finalizeLoRaFrame();
    }

    int itemsToConsume = static_cast<int>(m_loRaSymbolSpan);

    if (std::abs(m_loRaSFOCum) > (1.0f / (2.0f * static_cast<float>(m_osFactor))))
    {
        const int step = std::signbit(m_loRaSFOCum) ? -1 : 1;
        itemsToConsume -= step;
        m_loRaSFOCum -= step * (1.0f / static_cast<float>(m_osFactor));
    }

    m_loRaSFOCum += m_loRaSFOHat;
    return std::max(1, itemsToConsume);
}

void MeshtasticDemodSink::applyChannelSettings(int channelSampleRate, int bandwidth, int channelFrequencyOffset, bool force)
{
    qDebug() << "MeshtasticDemodSink::applyChannelSettings:"
            << " channelSampleRate: " << channelSampleRate
            << " channelFrequencyOffset: " << channelFrequencyOffset
            << " bandwidth: " << bandwidth;

    if ((channelFrequencyOffset != m_channelFrequencyOffset) ||
        (channelSampleRate != m_channelSampleRate) || force)
    {
        m_nco.setFreq(-channelFrequencyOffset, channelSampleRate);
    }

    if ((channelSampleRate != m_channelSampleRate) ||
        (bandwidth != m_bandwidth) || force)
    {
        const int targetFrameSyncRate = std::max(1, bandwidth * static_cast<int>(m_osFactor));
        // Keep the anti-alias/channel filter narrow around the configured LoRa bandwidth.
        // A too-wide cutoff destabilizes preamble bin tracking in DETECT.
        m_interpolator.create(16, channelSampleRate, m_bandwidth / 1.9f);
        m_interpolatorDistance = (Real) channelSampleRate / (Real) targetFrameSyncRate;
        m_sampleDistanceRemain = 0;
        m_osCounter = 0;
        qDebug() << "MeshtasticDemodSink::applyChannelSettings: m_interpolator.create:"
            << " m_interpolatorDistance: " << m_interpolatorDistance
            << " targetFrameSyncRate: " << targetFrameSyncRate
            << " osFactor: " << m_osFactor;
    }

    m_channelSampleRate = channelSampleRate;
    m_bandwidth = bandwidth;
    m_channelFrequencyOffset = channelFrequencyOffset;
}

void MeshtasticDemodSink::applySettings(const MeshtasticDemodSettings& settings, bool force)
{
    qDebug() << "MeshtasticDemodSink::applySettings:"
            << " m_inputFrequencyOffset: " << settings.m_inputFrequencyOffset
            << " m_bandwidthIndex: " << settings.m_bandwidthIndex
            << " m_spreadFactor: " << settings.m_spreadFactor
            << " m_rgbColor: " << settings.m_rgbColor
            << " m_title: " << settings.m_title
            << " force: " << force;

    const unsigned int desiredFFTInterpolation = m_loRaFFTInterpolation;
    const bool fftInterpChanged = desiredFFTInterpolation != m_fftInterpolation;

    if ((settings.m_spreadFactor != m_settings.m_spreadFactor)
     || (settings.m_deBits != m_settings.m_deBits)
     || fftInterpChanged
     || force)
    {
        m_fftInterpolation = desiredFFTInterpolation;
        initSF(settings.m_spreadFactor, settings.m_deBits);
    }

    const unsigned int configuredPreamble = settings.m_preambleChirps > 0U
        ? settings.m_preambleChirps
        : m_minRequiredPreambleChirps;
    const unsigned int targetRequired = configuredPreamble > 3U
        ? (configuredPreamble - 3U)
        : m_minRequiredPreambleChirps;
    m_requiredPreambleChirps = std::max(
        m_minRequiredPreambleChirps,
        std::min(targetRequired, m_maxRequiredPreambleChirps)
    );
    qDebug() << "MeshtasticDemodSink::applySettings:"
            << " requiredPreambleChirps: " << m_requiredPreambleChirps
            << " configuredPreamble: " << settings.m_preambleChirps
            << " fftInterpolation: " << m_fftInterpolation;

    m_settings = settings;
    m_loRaRequiredUpchirps = m_requiredPreambleChirps;
    m_loRaUpSymbToUse = (m_loRaRequiredUpchirps > 0U) ? static_cast<int>(m_loRaRequiredUpchirps - 1U) : 0;
    m_loRaPreambleVals.assign(m_loRaRequiredUpchirps, 0);
    m_loRaPreambleRaw.assign(m_nbSymbols * m_loRaRequiredUpchirps, Complex{0.0f, 0.0f});
    m_loRaPreambleUpchirps.assign(m_nbSymbols * m_loRaRequiredUpchirps, Complex{0.0f, 0.0f});
    m_loRaPreambleRawUp.assign((m_settings.m_preambleChirps + 3U) * m_loRaSymbolSpan, Complex{0.0f, 0.0f});
    m_loRaCFOFracCorrec.assign(m_nbSymbols, Complex{1.0f, 0.0f});
    m_loRaPayloadDownchirp.assign(m_nbSymbols, Complex{1.0f, 0.0f});
    m_loRaSymbCorr.assign(m_nbSymbols, Complex{0.0f, 0.0f});
    m_loRaNetIdSamp.assign((m_loRaSymbolSpan * 5U) / 2U + m_loRaSymbolSpan, Complex{0.0f, 0.0f});
    m_loRaAdditionalSymbolSamp.assign(m_loRaSymbolSpan * 2U, Complex{0.0f, 0.0f});
    resetLoRaFrameSync();
}