/*
 * rtl-sdr, turns your Realtek RTL2832 based DVB dongle into a SDR receiver
 * Copyright (C) 2012 by Steve Markgraf <steve@steve-m.de>
 * Copyright (C) 2012 by Hoernchen <la@tfc-server.de>
 * Copyright (C) 2012 by Kyle Keen <keenerd@gmail.com>
 * Copyright (C) 2013 by Elias Oenal <EliasOenal@gmail.com>
 * Copyright (C) 2014 by Kyle Keen <keenerd@gmail.com>
 * Copyright (C) 2025 by arancormonk <180709949+arancormonk@users.noreply.github.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, either version 2 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 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 <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <strings.h>
#include <pthread.h>
#include <unistd.h>
#include <sched.h>
#include <atomic>
#include <rtl-sdr.h>
#include "dsd.h"

/* Optional SIMD intrinsics for USB byte->int16 widening */
#if defined(__x86_64__) || defined(__i386__)
#include <immintrin.h>
#include <emmintrin.h>
#include <tmmintrin.h>
#endif
#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__)
#include <arm_neon.h>
#endif

/* Runtime CPU feature detection headers */
#if defined(__x86_64__) || defined(__i386__)
#include <cpuid.h>
#endif
#if defined(__linux__)
#include <sys/auxv.h>
#ifndef AT_HWCAP
#define AT_HWCAP 16
#endif
#endif

/*
 * Environment variables (runtime configuration)
 * Set in your shell before launching dsd-fme, e.g.:
 *   $ export DSD_FME_RESAMP=48000
 *   $ export DSD_FME_FLL=1 DSD_FME_FLL_ALPHA=150 DSD_FME_FLL_BETA=15
 *   $ export DSD_FME_TED=1 DSD_FME_TED_SPS=10 DSD_FME_TED_GAIN=96
 *   $ export DSD_FME_AUDIO_LPF=3000
 *   $ ./build/dsd-fme ...
 *
 * Realtime scheduling and CPU affinity
 * - DSD_FME_RT_SCHED
 *     Enable best-effort realtime scheduling (SCHED_FIFO). Requires CAP_SYS_NICE or root.
 *     Values: "1" to enable, unset/other to disable. Default: disabled.
 * - DSD_FME_RT_PRIO_USB | DSD_FME_RT_PRIO_DONGLE | DSD_FME_RT_PRIO_DEMOD
 *     Optional per-thread priorities (1..99, clamped to system limits). Used only if RT_SCHED=1.
 *     Example: export DSD_FME_RT_PRIO_DEMOD=85
 * - DSD_FME_CPU_USB | DSD_FME_CPU_DONGLE | DSD_FME_CPU_DEMOD
 *     Optional CPU core pinning for each thread. Integer CPU id (>=0). Example: export DSD_FME_CPU_DEMOD=2
 *
 * Frontend/decimation/upsampling
 * - DSD_FME_HB_DECIM
 *     Use half-band FIR decimator cascade (fast, good response) instead of legacy CIC-like path.
 *     Values: 1 enable, 0 disable. Default: 1 (enabled).
 * - DSD_FME_COMBINE_ROT
 *     Combine 90° IQ rotation with USB byte→int16 widening in one pass when offset tuning is off.
 *     Values: 1 enable, 0 disable. Default: 1 (enabled).
 * - DSD_FME_UPSAMPLE_FP
 *     Use fixed-point arithmetic in legacy linear upsampler for lower CPU/divisions.
 *     Values: 1 enable, 0 disable. Default: 1 (enabled).
 *
 * Rational resampler (polyphase upfirdn L/M)
 * - DSD_FME_RESAMP
 *     Target output sample rate in Hz. Enables L/M resampler when set.
 *     Values: "off" or "0" to disable; integer Hz (e.g., 48000) to enable. Default: 48000 (enabled).
 *
 * Residual CFO frequency-locked loop (FLL)
 * - DSD_FME_FLL
 *     Enable residual carrier frequency correction.
 *     Values: "1" or unset to enable; other values disable. Default: enabled.
 * - DSD_FME_FLL_LUT
 *     Use higher-quality quarter-wave sine LUT mixer for FLL rotation.
 *     Values: 1 enable, 0/empty disable. Default: 0 (disabled; fast piecewise approx).
 * - DSD_FME_FLL_ALPHA, DSD_FME_FLL_BETA
 *     Proportional and integral gains (Q15 fixed-point, ~value/32768). Typical small values.
 *     Defaults: ALPHA=100 (~0.003), BETA=10 (~0.0003). May be adjusted for digital modes if not set.
 *
 * Gardner timing error detector (TED)
 * - DSD_FME_TED
 *     Enable lightweight fractional-delay timing correction. Generally off for analog FM.
 *     Values: 1 enable, else disabled. Default: 0 (disabled). For certain digital modes, defaults are adjusted
 *     only if envs are not provided (still off unless forced via DSD_FME_TED=1).
 * - DSD_FME_TED_SPS
 *     Nominal samples-per-symbol (integer). If unset and a digital mode is active, it is derived from output rate.
 *     Default: 10.
 * - DSD_FME_TED_GAIN
 *     Small loop gain (Q20). Default: 64; for common digital modes may default to 96 when not provided.
 * - DSD_FME_TED_FORCE
 *     Force TED to run for FM/C4FM paths where it is normally skipped. Values: 1 enable, else disabled. Default: 0.
 *
 * Audio processing
 * - DSD_FME_DEEMPH
 *     Post-demod deemphasis time constant. Applies only when the active demod preset enables deemphasis.
 *     Values: "75" (75µs, default), "50" (50µs), "nfm" (~750µs), "off" (disable).
 * - DSD_FME_AUDIO_LPF
 *     Optional one-pole low-pass filter after demod. Approximate cutoff in Hz.
 *     Values: "off" or "0" to disable; integer (e.g., 3000, 5000) to enable. Default: off.
 *
 * Intra-block multithreading
 * - DSD_FME_MT
 *     Enable a minimal 2-thread worker pool for certain CPU-heavy inner loops.
 *     Values: 1 enable, else disabled. Default: 0 (disabled).
 */

#define DEFAULT_SAMPLE_RATE		48000
#define DEFAULT_BUF_LENGTH		(1 * 16384)
#define MAXIMUM_OVERSAMPLE		16
#define MAXIMUM_BUF_LENGTH		(MAXIMUM_OVERSAMPLE * DEFAULT_BUF_LENGTH)
#define AUTO_GAIN			-100
#define BUFFER_DUMP		4096

#define FREQUENCIES_LIMIT		  1000

/* Clamp for bandwidth upsampling multiplier to avoid extreme expansion */
#define MAX_BANDWIDTH_MULTIPLIER 8

static int lcm_post[17] = {1,1,1,3,1,5,3,7,1,9,5,11,3,13,7,15,1};
static int ACTUAL_BUF_LENGTH;

static const double kPi = 3.14159265358979323846;

#if defined(__GNUC__) || defined(__clang__)
#define DSD_FME_PRAGMA(x) _Pragma(#x)
#define DSD_FME_IVDEP DSD_FME_PRAGMA(GCC ivdep)
/**
 * Hint that a pointer is aligned to a compile-time boundary for vectorization.
 *
 * This is a lightweight wrapper over compiler intrinsics to improve
 * auto-vectorization by promising the compiler that the pointer meets the
 * specified alignment. Use with care and only when the alignment guarantee
 * is actually met.
 *
 * @tparam T Element type of the pointer.
 * @param p  Pointer to memory that is at least `align_unused` aligned.
 * @param align_unused Alignment in bytes (ignored at runtime; for readability).
 * @return Pointer `p` with alignment assumption applied.
 */
template <typename T>
static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) {
    return (T*)__builtin_assume_aligned(p, 64);
}
#else
#define DSD_FME_IVDEP
/**
 * See aligned variant: noop fallback when compiler does not support alignment
 * assumptions.
 * @tparam T Element type of the pointer.
 * @param p  Pointer to return as-is.
 * @param align_unused Unused parameter for signature compatibility.
 * @return Pointer `p` unchanged.
 */
template <typename T>
static inline T* assume_aligned_ptr(T* p, size_t /*align_unused*/) {
    return p;
}
#endif
#ifndef DSD_FME_ALIGN
#define DSD_FME_ALIGN 64
#endif

/* Compiler-friendly restrict qualifier */
#if defined(__GNUC__) || defined(__clang__)
#define DSD_FME_RESTRICT __restrict__
#else
#define DSD_FME_RESTRICT
#endif

static int *atan_lut = NULL;
static int atan_lut_size = 131072; /* 512 KB */
static int atan_lut_coef = 8;
static pthread_once_t atan_lut_once = PTHREAD_ONCE_INIT;
static pthread_mutex_t atan_lut_mutex = PTHREAD_MUTEX_INITIALIZER;
/* Optional quarter-wave sine LUT for FLL rotator (Q15). */
static int16_t fll_qsine_q15_lut[1025]; /* 0..pi/2 in 1024 steps, +1 guard for exact pi/2 */
static pthread_once_t fll_lut_once = PTHREAD_ONCE_INIT;
static int fll_lut_enabled = 0; /* DSD_FME_FLL_LUT (0 default: use fast approx) */
/* Debug/compat toggles via env */
static int combine_rotate_enabled = 1;     /* DSD_FME_COMBINE_ROT (1 default) */
static int upsample_fixedpoint_enabled = 1;/* DSD_FME_UPSAMPLE_FP (1 default) */

/* Forward declaration for runtime SIMD dispatch initializer */
static void dsd_fme_init_runtime_dispatch(void);
static void dsd_fme_init_runtime_dispatch_once(void);
static pthread_once_t dsd_fme_dispatch_once_control = PTHREAD_ONCE_INIT;

typedef void (*dsd_fme_widen_fn)(const unsigned char*, int16_t*, uint32_t);
typedef void (*dsd_fme_widen_rot_fn)(const unsigned char*, int16_t*, uint32_t);
static dsd_fme_widen_fn     g_widen_impl = NULL;
static dsd_fme_widen_rot_fn g_widen_rot_impl = NULL;

/**
 * Public wrapper that lazy-initializes runtime dispatch and widens u8 to s16
 * centered at 127.
 *
 * @param src Source buffer of unsigned bytes (I/Q interleaved).
 * @param dst Destination int16 buffer.
 * @param len Number of bytes in src to process.
 */
static inline void widen_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src,
    int16_t * DSD_FME_RESTRICT dst, uint32_t len)
{
    if (!g_widen_impl) dsd_fme_init_runtime_dispatch();
    g_widen_impl(src, dst, len);
}

/**
 * Public wrapper that lazy-initializes runtime dispatch and performs 90° IQ
 * rotation combined with widen u8→s16 centered at 127.
 *
 * @param src Source buffer of unsigned bytes (I/Q interleaved).
 * @param dst Destination int16 buffer.
 * @param len Number of bytes in src to process.
 */
static inline void widen_rotate90_u8_to_s16_bias127(const unsigned char * DSD_FME_RESTRICT src,
    int16_t * DSD_FME_RESTRICT dst, uint32_t len)
{
    if (!g_widen_rot_impl) dsd_fme_init_runtime_dispatch();
    g_widen_rot_impl(src, dst, len);
}

/**
 * Scalar fallback: widen u8 to s16 centered at 127.
 *
 * @param src Source buffer of unsigned bytes.
 * @param dst Destination int16 buffer.
 * @param len Number of bytes to process.
 */
static inline void widen_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src,
    int16_t * DSD_FME_RESTRICT dst, uint32_t len)
{
    uint32_t i = 0;
    /* Scalar conversion: (u8 - 127) -> s16 */
    for (; i < len; i++) {
        dst[i] = (int16_t)src[i] - 127;
    }
}

/**
 * Scalar widening that subtracts 128 instead of 127.
 * Intended to pair with legacy byte-wise rotate_90(u8) which performs 255-x
 * negation so that overall effect equals correct centered negation (127-x).
 *
 * @param src Source buffer of unsigned bytes.
 * @param dst Destination int16 buffer.
 * @param len Number of bytes to process.
 */
static inline void widen_u8_to_s16_bias128_scalar(const unsigned char * DSD_FME_RESTRICT src,
    int16_t * DSD_FME_RESTRICT dst, uint32_t len)
{
    uint32_t i = 0;
    for (; i < len; i++) {
        dst[i] = (int16_t)src[i] - 128;
    }
}

/**
 * Combined 90° rotation (1, j, -1, -j) + widen (u8→s16 centered at 127).
 * Processes 4 IQ samples per iteration to avoid branches.
 *
 * @param src Source buffer of unsigned bytes (I/Q interleaved).
 * @param dst Destination int16 buffer.
 * @param len Number of bytes in src to process.
 */
static inline void widen_rotate90_u8_to_s16_bias127_scalar(const unsigned char * DSD_FME_RESTRICT src,
    int16_t * DSD_FME_RESTRICT dst, uint32_t len)
{
    uint32_t i = 0;
    for (; i + 8 <= len; i += 8) {
        int16_t i0 = (int16_t)src[i + 0] - 127;
        int16_t q0 = (int16_t)src[i + 1] - 127;
        dst[i + 0] = i0;
        dst[i + 1] = q0;

        int16_t i1 = (int16_t)src[i + 2] - 127;
        int16_t q1 = (int16_t)src[i + 3] - 127;
        dst[i + 2] = (int16_t)(-q1);
        dst[i + 3] = i1;

        int16_t i2 = (int16_t)src[i + 4] - 127;
        int16_t q2 = (int16_t)src[i + 5] - 127;
        dst[i + 4] = (int16_t)(-i2);
        dst[i + 5] = (int16_t)(-q2);

        int16_t i3 = (int16_t)src[i + 6] - 127;
        int16_t q3 = (int16_t)src[i + 7] - 127;
        dst[i + 6] = q3;
        dst[i + 7] = (int16_t)(-i3);
    }
    /* Tail: apply rotation pattern for remaining up to 6 samples */
    if (i < len) {
        uint32_t base = i;
        uint32_t rem = len - base;
        if (rem >= 2) {
            int16_t i0 = (int16_t)src[base + 0] - 127;
            int16_t q0 = (int16_t)src[base + 1] - 127;
            dst[base + 0] = i0;
            dst[base + 1] = q0;
        }
        if (rem >= 4) {
            int16_t i1 = (int16_t)src[base + 2] - 127;
            int16_t q1 = (int16_t)src[base + 3] - 127;
            dst[base + 2] = (int16_t)(-q1);
            dst[base + 3] = i1;
        }
        if (rem >= 6) {
            int16_t i2 = (int16_t)src[base + 4] - 127;
            int16_t q2 = (int16_t)src[base + 5] - 127;
            dst[base + 4] = (int16_t)(-i2);
            dst[base + 5] = (int16_t)(-q2);
        }
    }
}

#if defined(__GNUC__) || defined(__clang__)
#define DSD_FME_TARGET_ATTR(x) __attribute__((target(x)))
#else
#define DSD_FME_TARGET_ATTR(x)
#endif

#if defined(__x86_64__) || defined(__i386__)
/* AVX2 specializations */
/**
 * AVX2: widen unsigned bytes to signed 16-bit centered at 127.
 * @param src Source u8 buffer.
 * @param dst Destination s16 buffer.
 * @param len Number of bytes to process.
 */
static void DSD_FME_TARGET_ATTR("avx2") widen_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len)
{
    uint32_t i = 0;
    const __m256i bias256 = _mm256_set1_epi16(127);
    for (; i + 32 <= len; i += 32) {
        __m128i b0 = _mm_loadu_si128((const __m128i*)(src + i));
        __m128i b1 = _mm_loadu_si128((const __m128i*)(src + i + 16));
        __m256i lo = _mm256_cvtepu8_epi16(b0);
        __m256i hi = _mm256_cvtepu8_epi16(b1);
        lo = _mm256_sub_epi16(lo, bias256);
        hi = _mm256_sub_epi16(hi, bias256);
        _mm256_storeu_si256((__m256i*)(dst + i), lo);
        _mm256_storeu_si256((__m256i*)(dst + i + 16), hi);
    }
    for (; i < len; i++) dst[i] = (int16_t)src[i] - 127;
}

/**
 * AVX2: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127.
 * Tail elements are handled by a scalar helper to preserve the rotation pattern.
 * @param src Source u8 buffer (I/Q interleaved).
 * @param dst Destination s16 buffer.
 * @param len Number of bytes to process.
 */
static void DSD_FME_TARGET_ATTR("avx2") widen_rotate90_u8_to_s16_bias127_avx2(const unsigned char *src, int16_t *dst, uint32_t len)
{
    const __m256i shuffle = _mm256_setr_epi8(
        0, 1, 3, 2, 4, 5, 7, 6,  8, 9,11,10,12,13,15,14,
        0, 1, 3, 2, 4, 5, 7, 6,  8, 9,11,10,12,13,15,14);
    const __m256i mask_sel = _mm256_setr_epi16(
        0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF,
        0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF);
    const __m256i c127 = _mm256_set1_epi16(127);
    uint32_t i = 0;
    for (; i + 32 <= len; i += 32) {
        __m256i v8 = _mm256_loadu_si256((const __m256i*)(src + i));
        __m256i sh = _mm256_shuffle_epi8(v8, shuffle);
        __m128i sh_lo = _mm256_castsi256_si128(sh);
        __m128i sh_hi = _mm256_extracti128_si256(sh, 1);
        __m256i v16_lo = _mm256_cvtepu8_epi16(sh_lo);
        __m256i v16_hi = _mm256_cvtepu8_epi16(sh_hi);
        __m256i bs_lo = _mm256_sub_epi16(v16_lo, c127);
        __m256i bm_lo = _mm256_sub_epi16(c127,  v16_lo);
        __m256i bs_hi = _mm256_sub_epi16(v16_hi, c127);
        __m256i bm_hi = _mm256_sub_epi16(c127,  v16_hi);
        __m256i out_lo = _mm256_blendv_epi8(bs_lo, bm_lo, mask_sel);
        __m256i out_hi = _mm256_blendv_epi8(bs_hi, bm_hi, mask_sel);
        _mm256_storeu_si256((__m256i*)(dst + i), out_lo);
        _mm256_storeu_si256((__m256i*)(dst + i + 16), out_hi);
    }
    /* Tail: preserve rotation via scalar helper */
    if (i < len) {
        widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i);
    }
}

/**
 * SSE2: widen unsigned bytes to signed 16-bit centered at 127.
 */
static void DSD_FME_TARGET_ATTR("sse2") widen_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len)
{
    uint32_t i = 0;
    const __m128i bias = _mm_set1_epi16(127);
    const __m128i zero = _mm_setzero_si128();
    for (; i + 16 <= len; i += 16) {
        __m128i b = _mm_loadu_si128((const __m128i*)(src + i));
        __m128i lo = _mm_unpacklo_epi8(b, zero);
        __m128i hi = _mm_unpackhi_epi8(b, zero);
        lo = _mm_sub_epi16(lo, bias);
        hi = _mm_sub_epi16(hi, bias);
        _mm_storeu_si128((__m128i*)(dst + i), lo);
        _mm_storeu_si128((__m128i*)(dst + i + 8), hi);
    }
    for (; i < len; i++) dst[i] = (int16_t)src[i] - 127;
}

/**
 * SSE2: fallback rotate+widen via scalar since SSE2 lacks byte-wise shuffle.
 */
static void DSD_FME_TARGET_ATTR("sse2") widen_rotate90_u8_to_s16_bias127_sse2(const unsigned char *src, int16_t *dst, uint32_t len)
{
    /* Keep scalar logic for correctness without SSSE3 pshufb (SSE2 lacks byte shuffle). */
    widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len);
}
#endif /* x86 */

#if defined(__x86_64__) || defined(__i386__)
/**
 * SSSE3: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127.
 * Tail elements are handled by a scalar helper to preserve the rotation pattern.
 */
static void DSD_FME_TARGET_ATTR("ssse3") widen_rotate90_u8_to_s16_bias127_ssse3(const unsigned char *src, int16_t *dst, uint32_t len)
{
    uint32_t i = 0;
    const __m128i shuffle = _mm_setr_epi8(
        0, 1, 3, 2, 4, 5, 7, 6,  8, 9,11,10,12,13,15,14);
    const __m128i mask_sel = _mm_setr_epi16(
        0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF);
    const __m128i c127 = _mm_set1_epi16(127);
    const __m128i zero = _mm_setzero_si128();
    for (; i + 16 <= len; i += 16) {
        __m128i v8 = _mm_loadu_si128((const __m128i*)(src + i));
        __m128i sh = _mm_shuffle_epi8(v8, shuffle);
        __m128i v16_lo = _mm_unpacklo_epi8(sh, zero);
        __m128i v16_hi = _mm_unpackhi_epi8(sh, zero);
        __m128i bs_lo = _mm_sub_epi16(v16_lo, c127);
        __m128i bm_lo = _mm_sub_epi16(c127,  v16_lo);
        __m128i bs_hi = _mm_sub_epi16(v16_hi, c127);
        __m128i bm_hi = _mm_sub_epi16(c127,  v16_hi);
        __m128i out_lo = _mm_or_si128(_mm_and_si128(bm_lo, mask_sel), _mm_andnot_si128(mask_sel, bs_lo));
        __m128i out_hi = _mm_or_si128(_mm_and_si128(bm_hi, mask_sel), _mm_andnot_si128(mask_sel, bs_hi));
        _mm_storeu_si128((__m128i*)(dst + i), out_lo);
        _mm_storeu_si128((__m128i*)(dst + i + 8), out_hi);
    }
    if (i < len) {
        widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i);
    }
}
#endif /* x86 */

#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__)
/**
 * NEON: widen unsigned bytes to signed 16-bit centered at 127.
 */
static void widen_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len)
{
    uint32_t i = 0;
    for (; i + 16 <= len; i += 16) {
        uint8x16_t v = vld1q_u8(src + i);
        uint8x8_t v_lo = vget_low_u8(v);
        uint8x8_t v_hi = vget_high_u8(v);
        int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(v_lo));
        int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(v_hi));
        int16x8_t lo = vsubq_s16(v16_lo, vdupq_n_s16(127));
        int16x8_t hi = vsubq_s16(v16_hi, vdupq_n_s16(127));
        vst1q_s16(dst + i, lo);
        vst1q_s16(dst + i + 8, hi);
    }
    for (; i < len; i++) dst[i] = (int16_t)src[i] - 127;
}

/**
 * NEON: rotate (1,j,-1,-j) interleaved IQ and widen u8→s16 centered at 127.
 * Uses table lookup on aarch64; on ARMv7 (no vqtbl1q_u8) falls back to scalar.
 */
static void widen_rotate90_u8_to_s16_bias127_neon(const unsigned char *src, int16_t *dst, uint32_t len)
{
#if defined(__aarch64__)
    const uint8x16_t tbl_idx = {0,1,3,2,4,5,7,6, 8,9,11,10,12,13,15,14};
    const int16x8_t c127 = vdupq_n_s16(127);
    const uint16_t mpat[8] = {0x0000,0x0000,0xFFFF,0x0000,0xFFFF,0xFFFF,0x0000,0xFFFF};
    const uint16x8_t msel = vld1q_u16(mpat);
    uint32_t i = 0;
    for (; i + 16 <= len; i += 16) {
        uint8x16_t v = vld1q_u8(src + i);
        uint8x16_t sh = vqtbl1q_u8(v, tbl_idx);
        uint8x8_t sh_lo8 = vget_low_u8(sh);
        uint8x8_t sh_hi8 = vget_high_u8(sh);
        int16x8_t v16_lo = vreinterpretq_s16_u16(vmovl_u8(sh_lo8));
        int16x8_t v16_hi = vreinterpretq_s16_u16(vmovl_u8(sh_hi8));
        int16x8_t bs_lo = vsubq_s16(v16_lo, c127);
        int16x8_t bm_lo = vsubq_s16(c127,   v16_lo);
        int16x8_t bs_hi = vsubq_s16(v16_hi, c127);
        int16x8_t bm_hi = vsubq_s16(c127,   v16_hi);
        int16x8_t out_lo = vbslq_s16(msel, bm_lo, bs_lo);
        int16x8_t out_hi = vbslq_s16(msel, bm_hi, bs_hi);
        vst1q_s16(dst + i, out_lo);
        vst1q_s16(dst + i + 8, out_hi);
    }
    /* Tail: preserve rotation via scalar helper */
    if (i < len) {
        widen_rotate90_u8_to_s16_bias127_scalar(src + i, dst + i, len - i);
    }
#else
    /* ARMv7 NEON lacks vqtbl1q_u8; use scalar fallback for rotate+widen */
    widen_rotate90_u8_to_s16_bias127_scalar(src, dst, len);
#endif
}
#endif

/**
 * Runtime CPU feature detection and dispatch binding for widening/rotation.
 *
 * Detects available SIMD features (AVX2/SSSE3/SSE2/NEON) and binds the
 * function pointers `g_widen_impl` and `g_widen_rot_impl` accordingly.
 * Safe to call concurrently; guarded by pthread_once in the public wrapper.
 */
static void dsd_fme_init_runtime_dispatch_once(void)
{

#if defined(__x86_64__) || defined(__i386__)
    int use_avx2 = 0;
    int use_sse2 = 0;
    int use_ssse3 = 0;
    unsigned int eax=0, ebx=0, ecx=0, edx=0;
    if (__get_cpuid(1, &eax, &ebx, &ecx, &edx)) {
        use_sse2 = (edx & bit_SSE2) ? 1 : 0;
        use_ssse3 = (ecx & bit_SSSE3) ? 1 : 0;
        int osxsave = (ecx & bit_OSXSAVE) ? 1 : 0;
        int avx = (ecx & bit_AVX) ? 1 : 0;
        if (osxsave && avx) {
            uint32_t xcr0_lo=0, xcr0_hi=0;
            /* xgetbv ecx=0 is non-privileged on userland */
            __asm__ volatile ("xgetbv" : "=a"(xcr0_lo), "=d"(xcr0_hi) : "c"(0));
            uint64_t xcr0 = ((uint64_t)xcr0_hi << 32) | xcr0_lo;
            if ((xcr0 & 0x6) == 0x6) {
                unsigned int eax7=0, ebx7=0, ecx7=0, edx7=0;
                if (__get_cpuid_count(7, 0, &eax7, &ebx7, &ecx7, &edx7)) {
                    use_avx2 = (ebx7 & bit_AVX2) ? 1 : 0;
                }
            }
        }
    }
#if defined(__x86_64__)
    if (!use_sse2) use_sse2 = 1; /* mandatory on x86_64 */
#endif
#endif

#if defined(__aarch64__)
    int use_neon = 1; /* ASIMD mandatory */
#elif defined(__arm__)
    int use_neon = 0;
#if defined(__linux__)
    unsigned long hw = getauxval(AT_HWCAP);
    /* HWCAP_NEON may be undefined on some headers; fallback to known value 4096 */
#ifndef HWCAP_NEON
#define HWCAP_NEON 4096
#endif
    use_neon = (hw & HWCAP_NEON) ? 1 : 0;
#endif
#if !defined(__ARM_NEON) && !defined(__ARM_NEON__)
    (void)use_neon;
#endif
#endif

    /* Fallbacks */
    g_widen_impl = &widen_u8_to_s16_bias127_scalar;
    g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_scalar;

#if defined(__ARM_NEON) || defined(__ARM_NEON__) || defined(__aarch64__)
    if (use_neon) {
        g_widen_impl = &widen_u8_to_s16_bias127_neon;
        g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_neon;
        return;
    }
#endif

#if defined(__x86_64__) || defined(__i386__)
    if (use_avx2) {
        g_widen_impl = &widen_u8_to_s16_bias127_avx2;
        g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_avx2;
        return;
    }
    if (use_ssse3 || use_sse2) {
        if (use_sse2) g_widen_impl = &widen_u8_to_s16_bias127_sse2;
        if (use_ssse3) g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_ssse3;
        else           g_widen_rot_impl = &widen_rotate90_u8_to_s16_bias127_sse2;
        return;
    }
#endif
}

/**
 * Ensure SIMD dispatch is initialized (thread-safe, idempotent).
 */
static void dsd_fme_init_runtime_dispatch(void)
{
	/* Thread-safe, idempotent initialization */
	pthread_once(&dsd_fme_dispatch_once_control, dsd_fme_init_runtime_dispatch_once);
}

/**
 * Saturate 32-bit integer to 16-bit range.
 *
 * @param x Input 32-bit value.
 * @return Clamped 16-bit value in [-32768, 32767].
 */
static inline int16_t sat16(int32_t x)
{
    if (x > 32767) return 32767;
    if (x < -32768) return -32768;
    return (int16_t)x;
}

/* Runtime flag (default enabled). Set DSD_FME_HB_DECIM=0 to use legacy decimator */
static int use_halfband_decimator = 1;

/* 15-tap half-band low-pass coefficients, Q15 scaled.
   Odd-indexed taps are zero; center tap is 0.5 (16384). The remaining even taps
   sum to 0.5 to yield unity DC gain. Coefficients are symmetric. */
#define HB_TAPS 15
#define HB_HALF ((HB_TAPS - 1) / 2)
static const int16_t hb_q15_taps[HB_TAPS] = {
	-108,    0,  1800,    0,  -500,    0,  7000, 16384,
	 7000,   0,  -500,    0,  1800,    0,   -108
};

/**
 * Decimate one real channel by 2 using a half-band FIR with persistent left history.
 *
 * @param in   Pointer to real input samples.
 * @param in_len Number of real input samples.
 * @param out  Pointer to output buffer (size >= in_len/2).
 * @param hist Persistent history of length HB_TAPS-1 (left wing).
 * @return Number of output samples written (in_len/2).
 */
static inline int hb_decim2_real(const int16_t *in, int in_len, int16_t *out, int16_t *hist)
{
	const int hist_len = HB_TAPS - 1;
	/* Pad right side by repeating last sample to avoid needing future context */
	int16_t last = (in_len > 0) ? in[in_len - 1] : 0;
	/* For simplicity, operate via a small ringless window into a temp view using hist + in + right pad (virtually). */
	int out_len = in_len >> 1; /* floor */
	for (int n = 0; n < out_len; n++) {
		int center_idx = hist_len + (n << 1); /* position in the concatenated [hist | in] domain */
		/* Half-band optimization: only even taps and the center tap contribute (symmetric). */
		const int16_t c0 = hb_q15_taps[0];
		const int16_t c2 = hb_q15_taps[2];
		const int16_t c4 = hb_q15_taps[4];
		const int16_t c6 = hb_q15_taps[6];
		const int16_t c7 = hb_q15_taps[7]; /* center */
		auto get_sample = [&](int src_idx) -> int16_t {
			if (src_idx < hist_len) {
				return hist[src_idx];
			} else {
				int rel = src_idx - hist_len;
				return (rel < in_len) ? in[rel] : last;
			}
		};
		int16_t xc  = get_sample(center_idx);
		int16_t xm1 = get_sample(center_idx - 1);
		int16_t xp1 = get_sample(center_idx + 1);
		int16_t xm3 = get_sample(center_idx - 3);
		int16_t xp3 = get_sample(center_idx + 3);
		int16_t xm5 = get_sample(center_idx - 5);
		int16_t xp5 = get_sample(center_idx + 5);
		int16_t xm7 = get_sample(center_idx - 7);
		int16_t xp7 = get_sample(center_idx + 7);
		int64_t acc = 0;
		acc += (int32_t)c7 * (int32_t)xc;
		acc += (int32_t)c6 * (int32_t)(xm1 + xp1);
		acc += (int32_t)c4 * (int32_t)(xm3 + xp3);
		acc += (int32_t)c2 * (int32_t)(xm5 + xp5);
		acc += (int32_t)c0 * (int32_t)(xm7 + xp7);
		/* Q15 -> Q0 with rounding */
		acc += (1 << 14);
		int32_t y = (int32_t)(acc >> 15);
		out[n] = sat16(y);
	}
	/* Update history with the last (HB_TAPS-1) input samples for next call */
	if (in_len >= hist_len) {
		memcpy(hist, in + (in_len - hist_len), (size_t)hist_len * sizeof(int16_t));
	} else {
		/* Not enough samples: keep tail of previous hist and append current */
		int need = hist_len - in_len;
		if (need > 0) {
			/* shift left existing hist */
			memmove(hist, hist + in_len, (size_t)need * sizeof(int16_t));
		}
		memcpy(hist + need, in, (size_t)in_len * sizeof(int16_t));
	}
	return out_len;
}

/**
 * Fused interleaved complex half-band decimator. Decimates I and Q in one pass
 * without deinterleaving. Exploits the half-band property (zero odd taps) and
 * symmetry; Q15 taps with rounding preserve unity DC gain.
 *
 * @param in      Pointer to interleaved input samples (I,Q,I,Q,...).
 * @param in_len  Number of interleaved int16_t values in input.
 * @param out     Pointer to interleaved output buffer (size >= in_len/2).
 * @param hist_i  Persistent I-channel history of length HB_TAPS-1.
 * @param hist_q  Persistent Q-channel history of length HB_TAPS-1.
 * @return Interleaved output length (in_len/2).
 */
static inline int hb_decim2_complex_interleaved(const int16_t *in, int in_len, int16_t *out,
    int16_t *hist_i, int16_t *hist_q)
{
    const int hist_len = HB_TAPS - 1;
    int ch_len = in_len >> 1; /* per-channel samples */
    int out_ch_len = ch_len >> 1; /* decimated per-channel */
    if (out_ch_len <= 0) {
        return 0;
    }
    int16_t lastI = (ch_len > 0) ? in[in_len - 2] : 0;
    int16_t lastQ = (ch_len > 0) ? in[in_len - 1] : 0;
    for (int n = 0; n < out_ch_len; n++) {
        int center_idx = hist_len + (n << 1); /* per-channel index */
        /* Half-band optimization: only even taps and the center tap contribute (symmetric). */
        const int16_t c0 = hb_q15_taps[0];
        const int16_t c2 = hb_q15_taps[2];
        const int16_t c4 = hb_q15_taps[4];
        const int16_t c6 = hb_q15_taps[6];
        const int16_t c7 = hb_q15_taps[7]; /* center */
        auto get_iq = [&](int src_idx, int16_t &xi, int16_t &xq) {
            if (src_idx < hist_len) {
                xi = hist_i[src_idx];
                xq = hist_q[src_idx];
            } else {
                int rel = src_idx - hist_len;
                if (rel < ch_len) {
                    xi = in[(size_t)(rel << 1)];
                    xq = in[(size_t)(rel << 1) + 1];
                } else {
                    xi = lastI;
                    xq = lastQ;
                }
            }
        };
        int16_t ci, cq;
        int16_t im1, qm1, ip1, qp1;
        int16_t im3, qm3, ip3, qp3;
        int16_t im5, qm5, ip5, qp5;
        int16_t im7, qm7, ip7, qp7;
        get_iq(center_idx, ci, cq);
        get_iq(center_idx - 1, im1, qm1);
        get_iq(center_idx + 1, ip1, qp1);
        get_iq(center_idx - 3, im3, qm3);
        get_iq(center_idx + 3, ip3, qp3);
        get_iq(center_idx - 5, im5, qm5);
        get_iq(center_idx + 5, ip5, qp5);
        get_iq(center_idx - 7, im7, qm7);
        get_iq(center_idx + 7, ip7, qp7);
        int64_t accI = 0;
        int64_t accQ = 0;
        accI += (int32_t)c7 * (int32_t)ci;
        accQ += (int32_t)c7 * (int32_t)cq;
        accI += (int32_t)c6 * (int32_t)(im1 + ip1);
        accQ += (int32_t)c6 * (int32_t)(qm1 + qp1);
        accI += (int32_t)c4 * (int32_t)(im3 + ip3);
        accQ += (int32_t)c4 * (int32_t)(qm3 + qp3);
        accI += (int32_t)c2 * (int32_t)(im5 + ip5);
        accQ += (int32_t)c2 * (int32_t)(qm5 + qp5);
        accI += (int32_t)c0 * (int32_t)(im7 + ip7);
        accQ += (int32_t)c0 * (int32_t)(qm7 + qp7);
        accI += (1 << 14);
        accQ += (1 << 14);
        int32_t yI = (int32_t)(accI >> 15);
        int32_t yQ = (int32_t)(accQ >> 15);
        out[(size_t)(n << 1)]     = sat16(yI);
        out[(size_t)(n << 1) + 1] = sat16(yQ);
    }
    /* Update histories with last HB_TAPS-1 per-channel input samples */
    if (ch_len >= hist_len) {
        int start = ch_len - hist_len;
        for (int k = 0; k < hist_len; k++) {
            int rel = start + k;
            hist_i[k] = in[(size_t)(rel << 1)];
            hist_q[k] = in[(size_t)(rel << 1) + 1];
        }
    } else {
        int need = hist_len - ch_len;
        if (need > 0) {
            memmove(hist_i, hist_i + ch_len, (size_t)need * sizeof(int16_t));
            memmove(hist_q, hist_q + ch_len, (size_t)need * sizeof(int16_t));
        }
        for (int k = 0; k < ch_len; k++) {
            hist_i[need + k] = in[(size_t)(k << 1)];
            hist_q[need + k] = in[(size_t)(k << 1) + 1];
        }
    }
    return out_ch_len << 1; /* interleaved length */
}

static void atan_lut_once_init(void)
{
	int i;
	atan_lut = static_cast<int*>(malloc(atan_lut_size * sizeof(int)));
	if (atan_lut == NULL) {
		return;
	}
	for (i = 0; i < atan_lut_size; i++) {
		atan_lut[i] = (int) (atan((double) i / (1<<atan_lut_coef)) / kPi * (1<<14));
	}
}

/* Build quarter-wave sine LUT in Q15: sin(theta) where theta in [0, pi/2] */
static void fll_lut_once_init(void)
{
	for (int i = 0; i <= 1024; i++) {
		double theta = (kPi * 0.5) * ((double)i / 1024.0);
		int v = (int)lrint(sin(theta) * 32767.0);
		if (v >  32767) v =  32767;
		if (v < -32767) v = -32767;
		fll_qsine_q15_lut[i] = (int16_t)v;
}
}

/**
 * Compute sin/cos in Q15 from phase using a quarter-wave sine LUT.
 * The choice to use the LUT vs. a fast piecewise approximation is
 * made by the caller (see fll_mix_and_update).
 *
 * @param phase_q15 Phase accumulator (Q15, wrap at 2*pi -> 1<<15 scale).
 * @param c_out     [out] Cosine Q15.
 * @param s_out     [out] Sine Q15.
 */
static inline void fll_sin_cos_q15_from_phase_lut(int phase_q15, int16_t *c_out, int16_t *s_out)
{
	/* phase_q15 wraps at 1<<15 mapping to 2*pi */
	int p = phase_q15 & 0x7FFF; /* 0..32767 */
	int quad = p >> 13;         /* 0..3 */
	int r = p & 0x1FFF;         /* position within quadrant: 0..8191 */

	/* Helper to sample quarter-wave S(r) with r in [0..8192] using 1024-segment linear interp */
	auto sample_quarter = [](int r8192) -> int16_t {
        if (r8192 < 0) r8192 = 0;
        if (r8192 > 8192) r8192 = 8192;
        int idx = r8192 >> 3;          /* 0..1024 */
        int frac = r8192 & 7;          /* 0..7 */
        int16_t s0 = fll_qsine_q15_lut[idx];
        int16_t s1 = fll_qsine_q15_lut[(idx < 1024) ? (idx + 1) : 1024];
        int diff = (int)s1 - (int)s0;
        int interp = (int)s0 + ((diff * frac + 4) >> 3); /* rounded */
        if (interp >  32767) interp =  32767;
        if (interp < -32767) interp = -32767;
        return (int16_t)interp;
    };

	int16_t s_pos, c_pos;
	/* Cosine within quadrant uses complementary angle in the quarter-wave */
    switch (quad) {
    case 0: /* [0, pi/2) */
        s_pos = sample_quarter(r);
        c_pos = sample_quarter(8192 - r);
        *s_out = s_pos;
        *c_out = c_pos;
        break;
    case 1: /* [pi/2, pi) */
        s_pos = sample_quarter(8192 - r);
        c_pos = sample_quarter(r);
        *s_out = s_pos;
        *c_out = (int16_t)(-c_pos);
        break;
    case 2: /* [pi, 3pi/2) */
        s_pos = sample_quarter(r);
        c_pos = sample_quarter(8192 - r);
        *s_out = (int16_t)(-s_pos);
        *c_out = (int16_t)(-c_pos);
        break;
    default: /* 3: [3pi/2, 2pi) */
        s_pos = sample_quarter(8192 - r);
        c_pos = sample_quarter(r);
        *s_out = (int16_t)(-s_pos);
        *c_out = c_pos;
        break;
    }
}

//UDP -- keep for compatibility reasons
#include <netinet/in.h>
#include <arpa/inet.h>
static pthread_t socket_freq;

int rtl_bandwidth;
int bandwidth_multiplier;
int bandwidth_divisor = 48000; //divide bandwidth by this to get multiplier for the for j loop to queue.push

short int volume_multiplier;
short int port;
struct dongle_state
{
	int      exit_flag;
	pthread_t thread;
	rtlsdr_dev_t *dev;
	int      dev_index;
	uint32_t freq;
	uint32_t rate;
	int      gain;
	uint32_t buf_len;
	int      ppm_error;
	int      offset_tuning;
	int      direct_sampling;
	std::atomic<int> mute;
	struct demod_state *demod_target;
};

struct demod_state
{
	int      exit_flag;
	pthread_t thread;
	int16_t  *lowpassed;
	/* Scratch buffer for demod thread to read blocks from the input ring */
	/* Not a ring; callback writes directly into the global input ring. */
	alignas(DSD_FME_ALIGN) int16_t  input_cb_buf[MAXIMUM_BUF_LENGTH];
	int      lp_len;
	int16_t  lp_i_hist[10][6];
	int16_t  lp_q_hist[10][6];
	alignas(DSD_FME_ALIGN) int16_t  result[MAXIMUM_BUF_LENGTH];
	int16_t  droop_i_hist[9];
	int16_t  droop_q_hist[9];
	int      result_len;
	int      rate_in;
	int      rate_out;
	int      rate_out2;
	int      now_r, now_j;
	int      pre_r, pre_j;
	int      prev_index;
	int      downsample;    /* min 1, max 256 */
	int      post_downsample;
	int      output_scale;
	int      squelch_level, conseq_squelch, squelch_hits, terminate_on_squelch;
	/* Incremental, decimated RMS squelch estimator (power-domain, sqrt-free) */
	int64_t  squelch_running_power;
	int      squelch_decim_stride;
	int      squelch_decim_phase;
	int      squelch_window;
	int      downsample_passes;
	int      comp_fir_size;
	int      custom_atan;
	int      deemph, deemph_a;
	int      deemph_avg;
	/* Optional post-demod audio low-pass filter (one-pole) */
	int      audio_lpf_enable;
	int      audio_lpf_alpha;   /* Q15 alpha for one-pole LPF */
	int      audio_lpf_state;   /* state/output y[n-1] in Q0 */
	int      now_lpr;
	int      prev_lpr_index;
	int      dc_block, dc_avg;
	/* Half-band decimator state */
	int16_t  hb_workbuf[MAXIMUM_BUF_LENGTH];
	int16_t  hb_hist_i[10][HB_TAPS-1];
	int16_t  hb_hist_q[10][HB_TAPS-1];
	/* Reserved buffers for potential deinterleave path (currently unused) */
	alignas(DSD_FME_ALIGN) int16_t  hb_i_buf[MAXIMUM_BUF_LENGTH/2];
	alignas(DSD_FME_ALIGN) int16_t  hb_q_buf[MAXIMUM_BUF_LENGTH/2];
	alignas(DSD_FME_ALIGN) int16_t  hb_i_out[MAXIMUM_BUF_LENGTH/2];
	alignas(DSD_FME_ALIGN) int16_t  hb_q_out[MAXIMUM_BUF_LENGTH/2];
	/* Preallocated buffer for linear upsampler (bandwidth_multiplier) */
	alignas(DSD_FME_ALIGN) int16_t  upsample_buf[MAXIMUM_BUF_LENGTH * MAX_BANDWIDTH_MULTIPLIER];
	/* Polyphase rational resampler (L/M) state and output buffer */
	int      resamp_enabled;
	int      resamp_target_hz;     /* desired output sample rate */
	int      resamp_L;             /* upsample factor */
	int      resamp_M;             /* downsample factor */
	int      resamp_phase;         /* 0..L-1 accumulator */
	int      resamp_taps_len;      /* prototype taps length (padded to K*L) */
	int      resamp_taps_per_phase;/* K = ceil(taps_len/L) */
	int16_t *resamp_taps;          /* Q15 taps, length = K*L */
	int16_t *resamp_hist;          /* circular history, length = K */
	int      resamp_hist_head;     /* head index into circular history [0..K-1] */
	/* Output buffer for resampler (worst-case 4x expansion) */
	alignas(DSD_FME_ALIGN) int16_t  resamp_outbuf[MAXIMUM_BUF_LENGTH * 4];
	/* Residual CFO loop (FLL) state */
	int      fll_enabled;
	int      fll_alpha_q15;   /* proportional gain (Q15) */
	int      fll_beta_q15;    /* integral gain (Q15) */
	int      fll_freq_q15;    /* NCO frequency increment (Q15 radians/sample scaled) */
	int      fll_phase_q15;   /* NCO phase accumulator (wrap at 2*pi -> 1<<15 scale) */
	int      fll_prev_r;
	int      fll_prev_j;
	/* Timing error detector (Gardner) fractional-delay state */
	int      ted_enabled;
	int      ted_force;       /* allow forcing TED even for FM/C4FM paths */
	int      ted_gain_q20;    /* small gain (Q20) for stability */
	int      ted_sps;         /* nominal samples per symbol (e.g., 10 for 4800 sym/s at 48k) */
	int      ted_mu_q20;      /* fractional phase [0,1) in Q20 */
	/* Work buffer for timing-adjusted I/Q */
	alignas(DSD_FME_ALIGN) int16_t  timing_buf[MAXIMUM_BUF_LENGTH];
	/* Minimal 2-thread worker pool for intra-block parallelism */
	int      mt_enabled;
	int      mt_ready;
	pthread_t mt_threads[2];
	pthread_mutex_t mt_lock;
	pthread_cond_t  mt_cv;
	pthread_cond_t  mt_done_cv;
	int      mt_should_exit;
	int      mt_epoch;
	int      mt_completed_in_epoch;
	int      mt_posted_count;
	struct { void (*run)(void*); void *arg; } mt_tasks[2];
	int      mt_worker_id[2];
	struct { struct demod_state *s; int id; } mt_args[2];
	int      (*discriminator)(int, int, int, int);
	void     (*mode_demod)(struct demod_state*);
	/* Ready/condvar kept for cleanup compatibility; input ring is a global SPSC ring */
	pthread_cond_t ready; /* kept for cleanup compatibility; unused now */
	pthread_mutex_t ready_m;
	struct output_state *output_target;
};

/* Forward declarations for minimal worker pool helpers */
static void demod_mt_init(struct demod_state *s);
static void demod_mt_destroy(struct demod_state *s);
static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1);


struct demod_mt_worker_arg { struct demod_state *s; int id; };

/**
 * Worker thread procedure for the minimal 2-thread DEMOD pool.
 * Waits for posted tasks, executes them, and signals completion.
 *
 * @param arg Pointer to `demod_mt_worker_arg` with owning state and worker id.
 * @return NULL when the worker exits.
 */
static void *demod_mt_worker(void *arg)
{
	struct demod_mt_worker_arg *wa = (struct demod_mt_worker_arg*)arg;
	struct demod_state *s = wa->s;
	const int id = wa->id;
	int local_epoch = 0;
	for (;;) {
		pthread_mutex_lock(&s->mt_lock);
		while (!s->mt_should_exit && s->mt_epoch == local_epoch) {
			pthread_cond_wait(&s->mt_cv, &s->mt_lock);
		}
		if (s->mt_should_exit) {
			pthread_mutex_unlock(&s->mt_lock);
			break;
		}
		local_epoch = s->mt_epoch;
		void (*fn)(void*) = NULL;
		void *fn_arg = NULL;
		if (id < s->mt_posted_count) {
			fn = s->mt_tasks[id].run;
			fn_arg = s->mt_tasks[id].arg;
		}
		pthread_mutex_unlock(&s->mt_lock);
		if (fn) {
			fn(fn_arg);
		}
		pthread_mutex_lock(&s->mt_lock);
		s->mt_completed_in_epoch++;
		if (s->mt_completed_in_epoch >= s->mt_posted_count) {
			pthread_cond_signal(&s->mt_done_cv);
		}
		pthread_mutex_unlock(&s->mt_lock);
	}
	return NULL;
}

/**
 * Initialize the minimal 2-thread worker pool for intra-block tasks.
 * Enabled when `DSD_FME_MT=1` in the environment.
 *
 * @param s Demodulator state to initialize with worker threads.
 */
static void demod_mt_init(struct demod_state *s)
{
	const char *mt = getenv("DSD_FME_MT");
	s->mt_enabled = (mt && mt[0] == '1') ? 1 : 0;
	s->mt_should_exit = 0;
	s->mt_epoch = 0;
	s->mt_completed_in_epoch = 0;
	s->mt_posted_count = 0;
	if (!s->mt_enabled) {
		return;
	}
	pthread_mutex_init(&s->mt_lock, NULL);
	pthread_cond_init(&s->mt_cv, NULL);
	pthread_cond_init(&s->mt_done_cv, NULL);
	/* Start two workers */
	for (int i = 0; i < 2; i++) {
		s->mt_args[i].s = s;
		s->mt_args[i].id = i;
		pthread_create(&s->mt_threads[i], NULL, demod_mt_worker, (void*)&s->mt_args[i]);
	}
	fprintf(stderr, "Intra-block multithreading enabled (DSD_FME_MT=1), workers: 2.\n");
}

/**
 * Tear down the minimal worker pool created by demod_mt_init.
 *
 * @param s Demodulator state whose worker pool will be destroyed.
 */
static void demod_mt_destroy(struct demod_state *s)
{
	if (!s->mt_enabled) return;
	pthread_mutex_lock(&s->mt_lock);
	s->mt_should_exit = 1;
	pthread_cond_broadcast(&s->mt_cv);
	pthread_mutex_unlock(&s->mt_lock);
	for (int i = 0; i < 2; i++) {
		pthread_join(s->mt_threads[i], NULL);
	}
	pthread_cond_destroy(&s->mt_done_cv);
	pthread_cond_destroy(&s->mt_cv);
	pthread_mutex_destroy(&s->mt_lock);
}

/**
 * Post up to two tasks to the worker pool and wait for their completion.
 * If the pool is disabled, runs tasks synchronously on the caller thread.
 *
 * @param s  Demodulator state with worker pool.
 * @param f0 Task 0 function pointer (may be NULL).
 * @param a0 Task 0 argument.
 * @param f1 Task 1 function pointer (may be NULL).
 * @param a1 Task 1 argument.
 */
static void demod_mt_run_two(struct demod_state *s, void (*f0)(void*), void *a0, void (*f1)(void*), void *a1)
{
	if (!s->mt_enabled) {
		if (f0) f0(a0);
		if (f1) f1(a1);
		return;
	}
	pthread_mutex_lock(&s->mt_lock);
	s->mt_tasks[0].run = f0; s->mt_tasks[0].arg = a0;
	s->mt_tasks[1].run = f1; s->mt_tasks[1].arg = a1;
	s->mt_posted_count = (f1 != NULL) ? 2 : 1;
	s->mt_completed_in_epoch = 0;
	s->mt_epoch++;
	pthread_cond_broadcast(&s->mt_cv);
	while (s->mt_completed_in_epoch < s->mt_posted_count) {
		pthread_cond_wait(&s->mt_done_cv, &s->mt_lock);
	}
	pthread_mutex_unlock(&s->mt_lock);
}

struct output_state
{
	int      rate;
	int16_t  *buffer;
	size_t   capacity;
	std::atomic<size_t> head;
	std::atomic<size_t> tail;
	pthread_cond_t ready;
	pthread_cond_t space;
	pthread_mutex_t ready_m;
};

/* Simple SPSC ring for interleaved I/Q int16_t samples (input path) */
struct input_ring_state
{
	int16_t  *buffer;
	size_t   capacity;   /* in int16_t elements */
	std::atomic<size_t> head;
	std::atomic<size_t> tail;
	pthread_cond_t ready;
	pthread_mutex_t ready_m;
};

/**
 * Number of samples currently in the input ring.
 */
static inline size_t input_ring_used(const struct input_ring_state *r)
{
	size_t h = r->head.load();
	size_t t = r->tail.load();
	if (h >= t) return h - t;
	return r->capacity - (t - h);
}

/**
 * Number of free slots available for writing in the input ring.
 */
static inline size_t input_ring_free(const struct input_ring_state *r)
{
	return (r->capacity - 1) - input_ring_used(r);
}

/**
 * Check if the input ring is empty.
 */
static inline int input_ring_is_empty(const struct input_ring_state *r)
{
	return r->head.load() == r->tail.load();
}

/**
 * Clear the input ring head/tail indices.
 */
static inline void input_ring_clear(struct input_ring_state *r)
{
	r->tail.store(0);
	r->head.store(0);
}

/**
 * Reserve up to two contiguous writable regions totaling at least min_needed
 * (or less if near full).
 *
 * May drop oldest half when full to avoid blocking.
 *
 * @param r          Input ring buffer state.
 * @param min_needed Minimum samples requested for writing.
 * @param p1         [out] First writable region pointer or NULL.
 * @param n1         [out] First writable region length.
 * @param p2         [out] Second writable region pointer or NULL.
 * @param n2         [out] Second writable region length.
 * @return Total writable samples granted across regions.
 */
static int input_ring_reserve(struct input_ring_state *r, size_t min_needed,
                       int16_t **p1, size_t *n1, int16_t **p2, size_t *n2)
{
	size_t free_sp = input_ring_free(r);
	/* Producer must never advance consumer tail; if full, grant nothing */
	/* Provide up to min(free_sp, min_needed) across at most two regions */
	size_t grant = (min_needed < free_sp) ? min_needed : free_sp;
	size_t h = r->head.load();
	size_t first = r->capacity - h;
	if (first > grant) first = grant;
	*p1 = (first > 0) ? (r->buffer + h) : NULL;
	*n1 = first;
	*p2 = NULL;
	*n2 = 0;
	if (grant > first) {
		*p2 = r->buffer;
		*n2 = grant - first;
	}
	return (int)(*n1 + *n2);
}

/**
 * Commit produced samples and signal consumer on empty→non-empty transition.
 *
 * @param r         Input ring buffer state.
 * @param produced  Number of samples produced to commit.
 */
static void input_ring_commit(struct input_ring_state *r, size_t produced)
{
	if (produced == 0) return;
	int need_signal = input_ring_is_empty(r);
	size_t h = r->head.load();
	h += produced;
	if (h >= r->capacity) h %= r->capacity;
	r->head.store(h);
	if (need_signal) {
		pthread_mutex_lock(&r->ready_m);
		pthread_cond_signal(&r->ready);
		pthread_mutex_unlock(&r->ready_m);
	}
}

/**
 * Write up to count samples into the input ring buffer, dropping oldest half
 * when necessary to avoid blocking.
 *
 * @param r     Input ring buffer state.
 * @param data  Source samples to write.
 * @param count Number of samples to write.
 */
static void input_ring_write(struct input_ring_state *r, const int16_t *data, size_t count)
{
	int need_signal = input_ring_is_empty(r);
	while (count > 0 && !exitflag) {
		size_t free_sp = input_ring_free(r);
		if (free_sp == 0) {
			/* Ring full: to avoid racing the consumer, drop remainder */
			break;
		}
		size_t write_now = (count < free_sp) ? count : free_sp;
		size_t h = r->head.load();
		size_t first = r->capacity - h;
		if (first > write_now) first = write_now;
		memcpy(r->buffer + h, data, first * sizeof(int16_t));
		if (write_now > first) {
			memcpy(r->buffer, data + first, (write_now - first) * sizeof(int16_t));
			h = write_now - first;
		} else {
			h += first;
			if (h == r->capacity) h = 0;
		}
		r->head.store(h);
		data += write_now;
		count -= write_now;
	}
	if (need_signal) {
		pthread_mutex_lock(&r->ready_m);
		pthread_cond_signal(&r->ready);
		pthread_mutex_unlock(&r->ready_m);
	}
}

/**
 * Read up to max_count samples from input ring buffer, blocking with timeout
 * until data is available or exit is requested.
 *
 * @param r         Input ring buffer state.
 * @param out       Destination buffer for samples.
 * @param max_count Maximum number of samples to read.
 * @return Number of samples read (>=1), 0 if max_count is 0, or -1 on exit.
 */
static int input_ring_read_block(struct input_ring_state *r, int16_t *out, size_t max_count)
{
	if (max_count == 0) return 0;
	while (input_ring_is_empty(r)) {
		struct timespec ts;
		clock_gettime(CLOCK_REALTIME, &ts);
		ts.tv_nsec += 10L * 1000000L; /* 10ms */
		if (ts.tv_nsec >= 1000000000L) {
			ts.tv_sec += ts.tv_nsec / 1000000000L;
			ts.tv_nsec = ts.tv_nsec % 1000000000L;
		}
		pthread_mutex_lock(&r->ready_m);
		pthread_cond_timedwait(&r->ready, &r->ready_m, &ts);
		pthread_mutex_unlock(&r->ready_m);
		if (exitflag) return -1;
	}
	size_t available = input_ring_used(r);
	size_t read_now = (max_count < available) ? max_count : available;
	size_t t = r->tail.load();
	size_t first = r->capacity - t;
	if (first > read_now) first = read_now;
	memcpy(out, r->buffer + t, first * sizeof(int16_t));
	t += first;
	if (t == r->capacity) t = 0;
	if (read_now > first) {
		memcpy(out + first, r->buffer, (read_now - first) * sizeof(int16_t));
		t = read_now - first;
	}
	r->tail.store(t);
	return (int)read_now;
}
struct controller_state
{
	int      exit_flag;
	pthread_t thread;
	uint32_t freqs[FREQUENCIES_LIMIT];
	int      freq_len;
	int      freq_now;
	int      edge;
	int      wb_mode;
	pthread_cond_t hop;
	pthread_mutex_t hop_m;
};

struct dongle_state dongle;
struct demod_state demod;
struct output_state output;
struct controller_state controller;
static struct input_ring_state input_ring;

#define safe_cond_signal(n, m) pthread_mutex_lock(m); pthread_cond_signal(n); pthread_mutex_unlock(m)
#define safe_cond_wait(n, m) pthread_mutex_lock(m); pthread_cond_wait(n, m); pthread_mutex_unlock(m)

/**
 * Optionally enable realtime scheduling and set CPU affinity for the current
 * thread based on environment variables.
 *
 * When `DSD_FME_RT_SCHED=1`, attempts to switch the calling thread to
 * SCHED_FIFO with a priority derived from `DSD_FME_RT_PRIO_<ROLE>` if present.
 * If `DSD_FME_CPU_<ROLE>` is set to a valid CPU index, pins the thread to that
 * CPU.
 *
 * @param role Optional role label (e.g. "DEMOD", "DONGLE") used to look up
 *             per-role environment variables.
 */
static void maybe_set_thread_realtime_and_affinity(const char *role)
{
    const char *enable = getenv("DSD_FME_RT_SCHED");
    if (!enable || enable[0] != '1') {
        return;
    }

    /* Optional: role-specific priority (1..99) for SCHED_FIFO */
    int policy = SCHED_FIFO;
    struct sched_param sp;
    int pmax = sched_get_priority_max(policy);
    int pmin = sched_get_priority_min(policy);
    int def = (pmax > 10) ? (pmax - 10) : pmax; /* default near top, but safe */
    char envname[64];

    sp.sched_priority = def;
    if (role) {
        /* e.g., DSD_FME_RT_PRIO_DEMOD, DSD_FME_RT_PRIO_DONGLE */
        snprintf(envname, sizeof(envname), "DSD_FME_RT_PRIO_%s", role);
        const char *prio_str = getenv(envname);
        if (prio_str && prio_str[0] != '\0') {
            int pr = atoi(prio_str);
            if (pr < pmin) pr = pmin;
            if (pr > pmax) pr = pmax;
            sp.sched_priority = pr;
        }
    }

    if (pthread_setschedparam(pthread_self(), policy, &sp) != 0) {
        fprintf(stderr, "WARNING: Failed to set %s thread to SCHED_FIFO (needs CAP_SYS_NICE).\n", role ? role : "RT");
    } else {
        fprintf(stderr, "%s thread SCHED_FIFO priority set to %d.\n", role ? role : "RT", sp.sched_priority);
    }

    /* Optional: role-specific CPU affinity: DSD_FME_CPU_DEMOD / DSD_FME_CPU_DONGLE */
    if (role) {
        snprintf(envname, sizeof(envname), "DSD_FME_CPU_%s", role);
        const char *cpu_str = getenv(envname);
        if (cpu_str && cpu_str[0] != '\0') {
            int cpu = atoi(cpu_str);
            if (cpu >= 0) {
#if defined(__linux__) && !defined(__CYGWIN__)
                cpu_set_t cpuset;
                CPU_ZERO(&cpuset);
                CPU_SET((unsigned)cpu, &cpuset);
                if (pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset) != 0) {
                    fprintf(stderr, "WARNING: Failed to set CPU affinity for %s thread to CPU %d.\n", role, cpu);
                } else {
                    fprintf(stderr, "%s thread pinned to CPU %d.\n", role, cpu);
                }
#else
                (void)cpu;
                fprintf(stderr, "NOTICE: CPU affinity not supported on this platform.\n");
#endif
            }
        }
    }
}

/**
 * Number of samples currently in the output ring.
 *
 * @param o Output ring state.
 * @return Number of queued samples.
 */
static inline size_t ring_used(const struct output_state *o)
{
    /* Atomics policy: head/tail are atomics. We use default sequential
       consistency for simplicity. In an SPSC ring, this could be relaxed
       to acquire/release without changing behavior. */
    size_t h = o->head.load();
    size_t t = o->tail.load();
    if (h >= t) return h - t;
    return o->capacity - (t - h);
}

/**
 * Number of free slots available in the output ring.
 *
 * @param o Output ring state.
 * @return Number of writable samples before the ring becomes full.
 */
static inline size_t ring_free(const struct output_state *o)
{
    return (o->capacity - 1) - ring_used(o);
}

/**
 * Check if the output ring is empty.
 *
 * @param o Output ring state.
 * @return Non-zero if empty, zero otherwise.
 */
static inline int ring_is_empty(const struct output_state *o)
{
    /* See atomics note in ring_used() for ordering considerations. */
    return o->head.load() == o->tail.load();
}

/**
 * Clear the output ring head/tail indices.
 *
 * @param o Output ring state to clear.
 */
static inline void ring_clear(struct output_state *o)
{
    /* Clearing indices; with relaxed ordering this would be a release store. */
    o->tail.store(0);
    o->head.store(0);
}

/* Write up to count samples, blocking until space is available. Signals data availability after writes. */
static void ring_write(struct output_state *o, const int16_t *data, size_t count)
{
    int need_signal = ring_is_empty(o);
    while (count > 0 && !exitflag) {
        size_t free_sp = ring_free(o);
        if (free_sp == 0) {
            /* Wait for space with timeout to avoid indefinite blocking */
            struct timespec ts;
            clock_gettime(CLOCK_REALTIME, &ts);
            ts.tv_nsec += 50L * 1000000L; /* 50ms */
            if (ts.tv_nsec >= 1000000000L) {
                ts.tv_sec += ts.tv_nsec / 1000000000L;
                ts.tv_nsec = ts.tv_nsec % 1000000000L;
            }
            pthread_mutex_lock(&o->ready_m);
            pthread_cond_timedwait(&o->space, &o->ready_m, &ts);
            pthread_mutex_unlock(&o->ready_m);
            if (exitflag) break;
            continue;
        }
        size_t write_now = (count < free_sp) ? count : free_sp;
        size_t h = o->head.load();
        size_t first = o->capacity - h;
        if (first > write_now) first = write_now;
        memcpy(o->buffer + h, data, first * sizeof(int16_t));
        if (write_now > first) {
            memcpy(o->buffer, data + first, (write_now - first) * sizeof(int16_t));
            h = write_now - first;
        } else {
            h += first;
            if (h == o->capacity) h = 0;
        }
        o->head.store(h);
        data += write_now;
        count -= write_now;
    }
    if (need_signal) {
        safe_cond_signal(&o->ready, &o->ready_m);
    }
}

/* Same as ring_write but does not signal; caller decides when to signal */
static void ring_write_no_signal(struct output_state *o, const int16_t *data, size_t count)
{
    while (count > 0 && !exitflag) {
        size_t free_sp = ring_free(o);
        if (free_sp == 0) {
            /* Wait for space with timeout to avoid indefinite blocking */
            struct timespec ts;
            clock_gettime(CLOCK_REALTIME, &ts);
            ts.tv_nsec += 50L * 1000000L; /* 50ms */
            if (ts.tv_nsec >= 1000000000L) {
                ts.tv_sec += ts.tv_nsec / 1000000000L;
                ts.tv_nsec = ts.tv_nsec % 1000000000L;
            }
            pthread_mutex_lock(&o->ready_m);
            pthread_cond_timedwait(&o->space, &o->ready_m, &ts);
            pthread_mutex_unlock(&o->ready_m);
            if (exitflag) break;
            continue;
        }
        size_t write_now = (count < free_sp) ? count : free_sp;
        size_t h = o->head.load();
        size_t first = o->capacity - h;
        if (first > write_now) first = write_now;
        memcpy(o->buffer + h, data, first * sizeof(int16_t));
        if (write_now > first) {
            memcpy(o->buffer, data + first, (write_now - first) * sizeof(int16_t));
            h = write_now - first;
        } else {
            h += first;
            if (h == o->capacity) h = 0;
        }
        o->head.store(h);
        data += write_now;
        count -= write_now;
    }
}

/**
 * Write and signal consumer only if the ring transitions from empty to
 * non-empty. Reduces unnecessary wakeups while ensuring timely reads.
 *
 * @param o     Output ring buffer state.
 * @param data  Source samples to write.
 * @param count Number of samples to write.
 */
static void ring_write_signal_on_empty_transition(struct output_state *o, const int16_t *data, size_t count)
{
    int need_signal = ring_is_empty(o);
    ring_write_no_signal(o, data, count);
    if (need_signal) {
        safe_cond_signal(&o->ready, &o->ready_m);
    }
}

/**
 * Read one sample from the output ring, blocking with timeout until available
 * or exit requested.
 *
 * @param o    Output ring buffer state.
 * @param out  Destination for one sample.
 * @return 0 on success, -1 on exit.
 */
static int ring_read_one(struct output_state *o, int16_t *out)
{
    while (ring_is_empty(o)) {
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        ts.tv_nsec += 10L * 1000000L; /* 10ms */
        if (ts.tv_nsec >= 1000000000L) {
            ts.tv_sec += ts.tv_nsec / 1000000000L;
            ts.tv_nsec = ts.tv_nsec % 1000000000L;
        }
        pthread_mutex_lock(&o->ready_m);
        pthread_cond_timedwait(&o->ready, &o->ready_m, &ts);
        pthread_mutex_unlock(&o->ready_m);
        if (exitflag) return -1;
    }
    size_t t = o->tail.load();
    *out = o->buffer[t];
    t += 1;
    if (t == o->capacity) t = 0;
    o->tail.store(t);
    /* Signal space available for producer */
    safe_cond_signal(&o->space, &o->ready_m);
    return 0;
}

/**
 * Read up to max_count samples into out. Blocks until at least one sample is available or exit.
 * Signals producer space once after the batch.
 *
 * @param o         Output ring buffer state.
 * @param out       Destination buffer for samples.
 * @param max_count Maximum number of samples to read.
 * @return Number of samples read (>=1) or -1 on exit.
 */
static int ring_read_batch(struct output_state *o, int16_t *out, size_t max_count)
{
    if (max_count == 0) return 0;
    while (ring_is_empty(o)) {
        struct timespec ts;
        clock_gettime(CLOCK_REALTIME, &ts);
        ts.tv_nsec += 10L * 1000000L; /* 10ms */
        if (ts.tv_nsec >= 1000000000L) {
            ts.tv_sec += ts.tv_nsec / 1000000000L;
            ts.tv_nsec = ts.tv_nsec % 1000000000L;
        }
        pthread_mutex_lock(&o->ready_m);
        pthread_cond_timedwait(&o->ready, &o->ready_m, &ts);
        pthread_mutex_unlock(&o->ready_m);
        if (exitflag) return -1;
    }

    size_t available = ring_used(o);
    size_t read_now = (max_count < available) ? max_count : available;
    size_t t = o->tail.load();
    size_t first = o->capacity - t;
    if (first > read_now) first = read_now;
    memcpy(out, o->buffer + t, first * sizeof(int16_t));
    t += first;
    if (t == o->capacity) t = 0;
    if (read_now > first) {
        memcpy(out + first, o->buffer, (read_now - first) * sizeof(int16_t));
        t = read_now - first;
    }
    o->tail.store(t);
    /* Signal space available once for the whole batch */
    safe_cond_signal(&o->space, &o->ready_m);
    return (int)read_now;
}

/* {length, coef, coef, coef}  and scaled by 2^15
   for now, only length 9, optimal way to get +85% bandwidth */
#define CIC_TABLE_MAX 10
int cic_9_tables[][10] = {
	{0,},
	{9, -156,  -97, 2798, -15489, 61019, -15489, 2798,  -97, -156},
	{9, -128, -568, 5593, -24125, 74126, -24125, 5593, -568, -128},
	{9, -129, -639, 6187, -26281, 77511, -26281, 6187, -639, -129},
	{9, -122, -612, 6082, -26353, 77818, -26353, 6082, -612, -122},
	{9, -120, -602, 6015, -26269, 77757, -26269, 6015, -602, -120},
	{9, -120, -582, 5951, -26128, 77542, -26128, 5951, -582, -120},
	{9, -119, -580, 5931, -26094, 77505, -26094, 5931, -580, -119},
	{9, -119, -578, 5921, -26077, 77484, -26077, 5921, -578, -119},
	{9, -119, -577, 5917, -26067, 77473, -26067, 5917, -577, -119},
	{9, -199, -362, 5303, -25505, 77489, -25505, 5303, -362, -199},
};

/**
 * Rotate interleaved IQ bytes by 90 degrees in-place.
 *
 * 90° rotation sequence: 1+0j, 0+1j, -1+0j, 0-1j. Operates on u8
 * interleaved I/Q in-place. Negation is performed as (255 - x) to
 * approximate centered negation for subsequent widening by subtracting 128.
 * Assumes `len` is a multiple of 8 (four I/Q pairs per loop).
 *
 * @param buf Interleaved IQ byte buffer.
 * @param len Buffer length in bytes (processed in blocks of 8).
 */
void rotate_90(unsigned char *buf, uint32_t len)
{
	uint32_t i;
	unsigned char tmp;
	/* Process only full 8-byte blocks (4 IQ pairs) to avoid overrun */
	uint32_t full = len - (len % 8);
	for (i=0; i<full; i+=8) {
		/* uint8_t negation = 255 - x */
		tmp = 255 - buf[i+3];
		buf[i+3] = buf[i+2];
		buf[i+2] = tmp;

		buf[i+4] = 255 - buf[i+4];
		buf[i+5] = 255 - buf[i+5];

		tmp = 255 - buf[i+6];
		buf[i+6] = buf[i+7];
		buf[i+7] = tmp;
	}
}

/**
 * Simple boxcar low-pass accumulator with decimation on interleaved I/Q.
 * Accumulates I and Q independently over `downsample` input samples and
 * writes a single output (I,Q) pair per window. Scaling/normalization is
 * deferred; this function sums and decimates with saturation on writeback.
 *
 * @param d Demodulator state (uses lowpassed buffer and decimation state).
 */
void low_pass(struct demod_state *d)
{
	int i=0, i2=0;
	while (i < d->lp_len) {
		d->now_r += d->lowpassed[i];
		d->now_j += d->lowpassed[i+1];
		i += 2;
		d->prev_index++;
		if (d->prev_index < d->downsample) {
			continue;
		}
		/* Saturate accumulated sums when writing back to int16 */
		d->lowpassed[i2]   = sat16(d->now_r);
		d->lowpassed[i2+1] = sat16(d->now_j);
		d->prev_index = 0;
		d->now_r = 0;
		d->now_j = 0;
		i2 += 2;
	}
	d->lp_len = i2;
}

/**
 * Boxcar low-pass and decimate by step (no wraparound).
 * Length must be a multiple of step.
 *
 * @param signal2 In/out buffer of samples.
 * @param len     Length of input buffer.
 * @param step    Decimation factor.
 * @return New length after decimation.
 */
int low_pass_simple(int16_t *signal2, int len, int step)
{
	int i, i2, sum;
	if (step <= 0) return len;
	for(i=0; i + (step-1) < len; i+=step) {
		sum = 0;
		for(i2=0; i2<step; i2++) {
			sum += (int)signal2[i + i2];
		}
		// Normalize by step with rounding. Writes output at i/step index.
		int val = (sum >= 0) ? (sum + step/2) / step : -(((-sum) + step/2) / step);
		signal2[i/step] = (int16_t)val;
	}
	/* Duplicate the final sample to provide one-sample lookahead for callers
	   that expect at least one extra element. */
	int out_len = len / step;
	if (out_len > 0) {
		signal2[out_len] = signal2[out_len - 1];
	}
	return out_len;
}

/**
 * Simple square window FIR on real samples with decimation to rate_out2.
 *
 * @param s Demodulator state (uses result buffer and decimation state).
 */
void low_pass_real(struct demod_state *s)
{
	int i=0, i2=0;
	int16_t *r = assume_aligned_ptr(s->result, DSD_FME_ALIGN);
	int fast = (int)s->rate_out;
	int slow = s->rate_out2;
	/* Precompute fixed-point reciprocal of decimation factor to avoid per-sample division */
	int decim = (slow != 0) ? (fast / slow) : 1;
	if (decim < 1) decim = 1;
	const int kShiftLPR = 15; /* Q15 reciprocal */
	int recip_decim_q = (1 << kShiftLPR) / decim;
	DSD_FME_IVDEP
	while (i < s->result_len) {
		s->now_lpr += r[i];
		i++;
		s->prev_lpr_index += slow;
		if (s->prev_lpr_index < fast) {
			continue;
		}
		/* Multiply by reciprocal and shift instead of dividing by (fast/slow) */
		int64_t scaled = ((int64_t)s->now_lpr * recip_decim_q);
		r[i2] = (int16_t)(scaled >> kShiftLPR);
		s->prev_lpr_index -= fast;
		s->now_lpr = 0;
		i2 += 1;
	}
	s->result_len = i2;
}

/**
 * Fifth-order half-band-like decimator operating on a single real sequence.
 * Caller applies this separately to I and Q streams. Uses 6-tap state in
 * `hist` and writes decimated output in-place.
 *
 * @param data   In/out real data buffer (single channel).
 * @param length Input length (elements), processed in-place.
 * @param hist   Persistent history buffer of length >= 6.
 */
void fifth_order(int16_t *data, int length, int16_t *hist)
{
	int i;
	int16_t a, b, c, d, e, f;
	a = hist[1];
	b = hist[2];
	c = hist[3];
	d = hist[4];
	e = hist[5];
	f = data[0];
	/* a downsample should improve resolution, so don't fully shift */
	data[0] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
	for (i=4; i<length; i+=4) {
		a = c;
		b = d;
		c = e;
		d = f;
		e = data[i-2];
		f = data[i];
		data[i/2] = (a + (b+e)*5 + (c+d)*10 + f) >> 4;
	}
	/* archive */
	hist[0] = a;
	hist[1] = b;
	hist[2] = c;
	hist[3] = d;
	hist[4] = e;
	hist[5] = f;
}

/**
 * FIR filter with symmetric 9-tap coefficients (phase-saving implementation).
 *
 * @param data   In/out data buffer (interleaved step of 2 assumed).
 * @param length Number of input samples.
 * @param fir    Coefficient array (expects layout for length 9).
 * @param hist   History buffer used across calls.
 */
void generic_fir(int16_t *data, int length, int *fir, int16_t *hist)
{
	int d, temp, sum;
	for (d=0; d<length; d+=2) {
		temp = data[d];
		sum = 0;
		sum += (hist[0] + hist[8]) * fir[1];
		sum += (hist[1] + hist[7]) * fir[2];
		sum += (hist[2] + hist[6]) * fir[3];
		sum += (hist[3] + hist[5]) * fir[4];
		sum +=            hist[4]  * fir[5];
		data[d] = sum >> 15 ;
		hist[0] = hist[1];
		hist[1] = hist[2];
		hist[2] = hist[3];
		hist[3] = hist[4];
		hist[4] = hist[5];
		hist[5] = hist[6];
		hist[6] = hist[7];
		hist[7] = hist[8];
		hist[8] = temp;
	}
}

/**
 * Complex multiply using 32-bit intermediates (suitable for small magnitudes).
 * Defined for platforms lacking hardware float.
 *
 * @param ar Real part of a.
 * @param aj Imag part of a.
 * @param br Real part of b.
 * @param bj Imag part of b.
 * @param cr [out] Real part of result.
 * @param cj [out] Imag part of result.
 */
void multiply(int ar, int aj, int br, int bj, int *cr, int *cj)
{
	*cr = ar*br - aj*bj;
	*cj = aj*br + ar*bj;
}

/**
 * Complex multiply using 64-bit intermediates to prevent overflow.
 *
 * @param ar Real part of a.
 * @param aj Imag part of a.
 * @param br Real part of b.
 * @param bj Imag part of b.
 * @param cr [out] Real part of result (int64).
 * @param cj [out] Imag part of result (int64).
 */
static inline void multiply64(int ar, int aj, int br, int bj, int64_t *cr, int64_t *cj)
{
	*cr = (int64_t)ar * (int64_t)br - (int64_t)aj * (int64_t)bj;
	*cj = (int64_t)aj * (int64_t)br + (int64_t)ar * (int64_t)bj;
}

int polar_discriminant(int ar, int aj, int br, int bj)
{
	int64_t cr, cj;
	double angle;
	multiply64(ar, aj, br, -bj, &cr, &cj);
	angle = atan2((double)cj, (double)cr);
	return (int)(angle / kPi * (1<<14));
}

/**
 * Fast integer atan2 approximation pre-scaled for int16.
 *
 * @param y Imaginary component.
 * @param x Real component.
 * @return Angle where pi == 1<<14 (Q14 scaling).
 */
int fast_atan2(int y, int x)
{
	int yabs, angle;
	int pi4=(1<<12), pi34=3*(1<<12);  // note pi = 1<<14
	if (x==0 && y==0) {
		return 0;
	}
	yabs = y;
	if (yabs < 0) {
		yabs = -yabs;
	}
	if (x >= 0) {
		angle = pi4  - pi4 * (x-yabs) / (x+yabs);
	} else {
		angle = pi34 - pi4 * (x+yabs) / (yabs-x);
	}
	if (y < 0) {
		return -angle;
	}
	return angle;
}

/**
 * 64-bit safe fast atan2 approximation to avoid overflow.
 *
 * @param y Imaginary component (int64).
 * @param x Real component (int64).
 * @return Angle where pi == 1<<14 (Q14 scaling).
 */
int fast_atan2_64(int64_t y, int64_t x)
{
	int angle;
	int pi4=(1<<12), pi34=3*(1<<12);  /* note: pi = 1<<14 */
	int64_t yabs;
	if (x == 0 && y == 0) {
		return 0;
	}
	yabs = y;
	if (yabs < 0) {
		yabs = -yabs;
	}
	if (x >= 0) {
		/* denominator (x + yabs) cannot be zero here unless x==y==0 handled above */
		angle = (int)(pi4  - ( (int64_t)pi4 * (x - yabs) ) / (x + yabs));
	} else {
		/* denominator (yabs - x) > 0 */
		angle = (int)(pi34 - ( (int64_t)pi4 * (x + yabs) ) / (yabs - x));
	}
	if (y < 0) {
		return -angle;
	}
	return angle;
}

int polar_disc_fast(int ar, int aj, int br, int bj)
{
	int64_t cr, cj;
	multiply64(ar, aj, br, -bj, &cr, &cj);
	return fast_atan2_64(cj, cr);
}

/**
 * Initialize the fast arctangent lookup table used by the LUT discriminator.
 * Thread-safe and idempotent; subsequent calls are inexpensive.
 *
 * @return 0 on success, -1 on allocation failure.
 */
int atan_lut_init(void)
{
	/* Thread-safe, idempotent initialization */
	pthread_once(&atan_lut_once, atan_lut_once_init);
	if (atan_lut != NULL) {
		return 0;
	}
	/* If LUT was freed after once, allow re-init guarded by mutex */
	pthread_mutex_lock(&atan_lut_mutex);
	if (atan_lut == NULL) {
		atan_lut_once_init();
	}
	pthread_mutex_unlock(&atan_lut_mutex);
	return (atan_lut != NULL) ? 0 : -1;
}

/**
 * Free memory associated with the fast arctangent lookup table.
 * Safe to call multiple times.
 */
void atan_lut_free(void)
{
	pthread_mutex_lock(&atan_lut_mutex);
	if (atan_lut != NULL) {
		free(atan_lut);
		atan_lut = NULL;
	}
	pthread_mutex_unlock(&atan_lut_mutex);
}

/**
 * Polar discriminator using a lookup table for atan2 approximation.
 *
 * Multiplies sample b by the conjugate of sample a and estimates the phase
 * change via a LUT-backed atan2 approximation, returning a Q14-scaled angle.
 *
 * @param ar Real part of previous complex sample.
 * @param aj Imag part of previous complex sample.
 * @param br Real part of current complex sample.
 * @param bj Imag part of current complex sample.
 * @return Phase difference in Q14 where (pi == 1<<14).
 */
int polar_disc_lut(int ar, int aj, int br, int bj)
{
	int64_t cr, cj;
	int64_t x, x_abs;

	/* Ensure LUT is available; fall back if allocation failed */
	atan_lut_init();
	if (atan_lut == NULL) {
		multiply64(ar, aj, br, -bj, &cr, &cj);
		return fast_atan2_64(cj, cr);
	}

	multiply64(ar, aj, br, -bj, &cr, &cj);

	/* special cases */
	if (cr == 0 || cj == 0) {
		if (cr == 0 && cj == 0)
			{return 0;}
		if (cr == 0 && cj > 0)
			{return 1 << 13;}
		if (cr == 0 && cj < 0)
			{return -(1 << 13);}
		if (cj == 0 && cr > 0)
			{return 0;}
		if (cj == 0 && cr < 0)
			{return (1 << 14) - 1;}
	}

	/* real range -32768 - 32768 use 64x range -> absolute maximum: 2097152 */
	x = ((int64_t)cj << atan_lut_coef) / cr;
	x_abs = (x < 0) ? -x : x;

	if (x_abs >= (int64_t)atan_lut_size) {
		/* Preserve quadrant using both cr and cj signs */
		if (cr < 0) {
			return (cj >= 0) ? ((1 << 14) - 1) : (-(1 << 14) + 1);
		} else {
			return (cj >= 0) ? (1 << 13) : -(1 << 13);
		}
	}

	if (x > 0) {
		int val = (cj > 0) ? atan_lut[(int)x] : (atan_lut[(int)x] - (1<<14));
		if (val == (1 << 14)) { val = (1 << 14) - 1; }
		if (val == -(1 << 14)) { val = -(1 << 14) + 1; }
		return val;
	} else {
		int val = (cj > 0) ? ((1<<14) - atan_lut[(int)(-x)]) : (-atan_lut[(int)(-x)]);
		if (val == (1 << 14)) { val = (1 << 14) - 1; }
		if (val == -(1 << 14)) { val = -(1 << 14) + 1; }
		return val;
	}

	return 0;
}

/**
 * Perform FM discriminator on interleaved low-passed I/Q to produce audio PCM.
 * Uses the active discriminator configured in fm->discriminator.
 *
 * @param fm Demodulator state (uses lowpassed as input, writes to result).
 */
void fm_demod(struct demod_state *fm)
{
	int i, pcm;
	int16_t *lp = assume_aligned_ptr(fm->lowpassed, DSD_FME_ALIGN);
	int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN);
	/* Use selected discriminator from the very first sample */
	pcm = fm->discriminator(lp[0], lp[1], fm->pre_r, fm->pre_j);
	res[0] = (int16_t)pcm;
	DSD_FME_IVDEP
	for (i = 2; i < (fm->lp_len-1); i += 2) {
		pcm = fm->discriminator(lp[i], lp[i+1], lp[i-2], lp[i-1]);
		res[i/2] = (int16_t)pcm;
	}
	fm->pre_r = lp[fm->lp_len - 2];
	fm->pre_j = lp[fm->lp_len - 1];
	fm->result_len = fm->lp_len/2;
}

/**
 * Pass-through demodulator: copies low-passed samples to output unchanged.
 *
 * @param fm Demodulator state (copies lowpassed to result).
 */
void raw_demod(struct demod_state *fm)
{
	int i;
	for (i = 0; i < fm->lp_len; i++) {
		fm->result[i] = (int16_t)fm->lowpassed[i];
	}
	fm->result_len = fm->lp_len;
}

/**
 * Apply post-demod deemphasis IIR filter with Q15 coefficient.
 *
 * @param fm Demodulator state (reads/writes result, updates deemph_avg).
 */
void deemph_filter(struct demod_state *fm)
{
	int avg = fm->deemph_avg; /* per-instance state */
	int i, d;
	int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN);
	/* Q15 alpha = (1 - a), where a = exp(-1/(Fs*tau)) */
	const int kShiftDeemph = 15; /* Q15 */
	int alpha_q15 = fm->deemph_a;
	if (alpha_q15 < 0) alpha_q15 = 0;
	if (alpha_q15 > (1 << kShiftDeemph)) alpha_q15 = (1 << kShiftDeemph);
	/* Single-pole IIR: avg += (x - avg) * alpha */
	DSD_FME_IVDEP
	for (i = 0; i < fm->result_len; i++) {
		d = res[i] - avg;
		int64_t delta = (int64_t)d * (int64_t)alpha_q15;
		/* symmetric rounding */
		if (d > 0) {
			delta += (1LL << (kShiftDeemph - 1));
		} else if (d < 0) {
			delta -= (1LL << (kShiftDeemph - 1));
		}
		avg += (int)(delta >> kShiftDeemph);
		res[i] = (int16_t)avg;
	}
	fm->deemph_avg = avg; /* write back state */
}

/**
 * Apply a simple DC blocking (leaky integrator high-pass) filter to audio.
 *
 * @param fm Demodulator state (reads/writes result, updates dc_avg).
 */
void dc_block_filter(struct demod_state *fm)
{
	int i;
	/* Leaky integrator high-pass: dc += (x - dc) >> k; y = x - dc */
	int dc = fm->dc_avg;
	const int k = 11; /* cutoff ~ Fs / 2^k (k in 10..12) */
	int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN);
	DSD_FME_IVDEP
	for (i = 0; i < fm->result_len; i++) {
		int x = (int)res[i];
		dc += (x - dc) >> k;
		int y = x - dc;
		res[i] = sat16(y);
	}
	fm->dc_avg = dc;
}

/**
 * Optional light post-demod audio low-pass filter (one-pole IIR).
 * Implements: y[n] = y[n-1] + alpha * (x[n] - y[n-1]), where alpha is Q15 in
 * `fm->audio_lpf_alpha`.
 *
 * @param fm Demodulator state (reads/writes `result`, updates `audio_lpf_state`).
 */
static inline void audio_lpf_filter(struct demod_state *fm)
{
    if (!fm->audio_lpf_enable) return;
    int i;
    int16_t *res = assume_aligned_ptr(fm->result, DSD_FME_ALIGN);
    int y = fm->audio_lpf_state; /* Q0 */
    const int alpha_q15 = fm->audio_lpf_alpha; /* Q15 */
    const int kShift = 15;
    DSD_FME_IVDEP
    for (i = 0; i < fm->result_len; i++) {
        int x = (int)res[i];
        int d = x - y;
        int64_t delta = (int64_t)d * (int64_t)alpha_q15;
        /* symmetric rounding */
        if (d >= 0) delta += (1LL << (kShift - 1));
        else        delta -= (1LL << (kShift - 1));
        y += (int)(delta >> kShift);
        res[i] = (int16_t)y;
    }
    fm->audio_lpf_state = y;
}

/**
 * Mix lowpassed I/Q by NCO e^{j*phi}, update phase by `fll_freq_q15` per sample.
 * Phase and frequency are Q15 where a full turn (2*pi) maps to 1<<15.
 *
 * @param d Demodulator state (reads/writes `lowpassed`, updates `fll_phase_q15`).
 */
static inline void fll_mix_and_update(struct demod_state *d)
{
    if (!d->fll_enabled) return;
    int16_t *x = d->lowpassed;
    const int N = d->lp_len;
    int phase = d->fll_phase_q15;   /* Q15 wraps at 1<<15 ~ 2*pi */
    const int freq = d->fll_freq_q15; /* Q15 increment per sample */
    /* Optional: higher-quality quarter-wave LUT rotator (linear interp), enabled via DSD_FME_FLL_LUT=1. */
    if (fll_lut_enabled) {
    	/* Ensure LUT is initialized once before use */
    	pthread_once(&fll_lut_once, fll_lut_once_init);
    	for (int i = 0; i + 1 < N; i += 2) {
    		int16_t c, s;
    		fll_sin_cos_q15_from_phase_lut(phase, &c, &s);
    		int xr = x[i];
    		int xj = x[i+1];
    		int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15;
    		int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15;
    		x[i]   = (int16_t)yr;
    		x[i+1] = (int16_t)yj;
    		phase += freq;
    	}
    } else {
    	/* Fast LUT-free rotator: piecewise-linear sin/cos within quadrants. */
    	for (int i = 0; i + 1 < N; i += 2) {
    		int p = phase & 0x7FFF; /* 0..32767 */
    		int q = p >> 13; /* quadrant 0..3 */
    		int16_t c = 32767, s = 0;
    		int16_t r = (int16_t)(p & 0x1FFF); /* 0..8191 */
    		switch (q) {
    			case 0: c = 32767; s = (int16_t)((r * 4)); break;
    			case 1: c = (int16_t)(32767 - (r * 4)); s = 32767; break;
    			case 2: c = -32767; s = (int16_t)(32767 - (r * 4)); break;
    			default: c = (int16_t)(-32767 + (r * 4)); s = -32767; break;
    		}
    		int xr = x[i];
    		int xj = x[i+1];
    		int32_t yr = ((int32_t)xr * c + (int32_t)xj * s) >> 15;
    		int32_t yj = ((int32_t)xj * c - (int32_t)xr * s) >> 15;
    		x[i]   = (int16_t)yr;
    		x[i+1] = (int16_t)yj;
    		phase += freq;
    	}
    }
    d->fll_phase_q15 = phase & 0x7FFF;
}

/**
 * Estimate frequency error using a simple phase-difference discriminator and
 * update the FLL control in Q15. The proportional term is applied directly
 * and the integral action is realized by accumulating into `fll_freq_q15`.
 *
 * @param d Demodulator state (updates `fll_freq_q15` and `fll_phase_q15`).
 */
static inline void fll_update_error(struct demod_state *d)
{
    if (!d->fll_enabled) return;
    int16_t *x = d->lowpassed;
    const int N = d->lp_len;
    int alpha = d->fll_alpha_q15; /* Q15 */
    int beta  = d->fll_beta_q15;  /* Q15 */
    int prev_r = d->fll_prev_r;
    int prev_j = d->fll_prev_j;
    int32_t err_acc = 0;
    int count = 0;
    for (int i = 0; i + 1 < N; i += 2) {
        int r = x[i];
        int j = x[i+1];
        if (i > 0 || (prev_r != 0 || prev_j != 0)) {
            int e = polar_disc_fast(r, j, prev_r, prev_j); /* Q14 */
            err_acc += e;
            count++;
        }
        prev_r = r; prev_j = j;
    }
    d->fll_prev_r = prev_r;
    d->fll_prev_j = prev_j;
    if (count == 0) return;
    int32_t err = err_acc / count; /* Q14 */
    int32_t p = ( (int64_t)alpha * err ) >> 14; /* Q14 */
    int32_t iacc = ( (int64_t)beta  * err ) >> 14;
    int32_t df = p + iacc;
    d->fll_freq_q15 += (int)df; /* Q15 */
}

/**
 * Lightweight Gardner timing correction.
 * Uses linear interpolation between adjacent complex samples around the
 * nominal samples-per-symbol to reduce timing error; intended for digital
 * modes when enabled.
 *
 * @param d Demodulator state (may adjust `result` in-place).
 */
static inline void gardner_timing_adjust(struct demod_state *d)
{
    if (!d->ted_enabled || d->ted_sps <= 1) return;
    /* Guard: run TED only when we're near symbol rate to keep CPU low.
       Skip when samples-per-symbol is very high unless explicitly forced. */
    int sps = d->ted_sps;
    if (sps > 12 && !d->ted_force) return;
    int mu  = d->ted_mu_q20;    /* Q20 */
    int gain = d->ted_gain_q20; /* Q20 */
    int16_t *x = d->lowpassed;
    int16_t *y = d->timing_buf;
    const int N = d->lp_len;
    int out_n = 0;
    const int one = (1<<20);
    int mu_nom = one / (sps > 0 ? sps : 1); /* Q20 increment per complex sample */
    for (int n = 0; n + 3 < N; n += 2) {
        int a = n;       /* base complex sample index */
        int b = n + 2;   /* next complex sample index */
        if (b + 1 >= N) break;
        int frac = mu & (one - 1); /* 0..one-1 */
        int inv  = one - frac;
        /* Linear interpolation between x[a] and x[b] (complex) */
        int32_t ar = x[a];   int32_t aj = x[a+1];
        int32_t br = x[b];   int32_t bj = x[b+1];
        int32_t ir = (int32_t)(( (int64_t)inv * ar + (int64_t)frac * br ) >> 20);
        int32_t ij = (int32_t)(( (int64_t)inv * aj + (int64_t)frac * bj ) >> 20);
        y[out_n++] = (int16_t)ir;
        y[out_n++] = (int16_t)ij;

        /* Gardner error using previous and next symbol-spaced samples */
        int km1 = a - 2; if (km1 < 0) km1 = 0;
        int kp1 = b + 2; if (kp1 + 1 >= N) { kp1 = b; }
        int16_t xr1 = x[kp1];
        int16_t xj1 = x[kp1+1];
        int16_t xrm = x[km1];
        int16_t xjm = x[km1+1];
        int16_t dr = xr1 - xrm;
        int16_t dj = xj1 - xjm;
        int32_t e = (int32_t)dr * (int32_t)ir + (int32_t)dj * (int32_t)ij; /* Q0 */

        /* Update fractional phase: nominal advance + small correction */
        int64_t corr = ((int64_t)gain * (int64_t)e) >> 15; /* scale */
        mu += mu_nom + (int)corr;
        /* Wrap mu to [0, one) */
        if (mu >= one) mu -= one;
        if (mu < 0) mu += one;
    }
    if (out_n >= 2) {
        memcpy(d->lowpassed, y, (size_t)out_n * sizeof(int16_t));
        d->lp_len = out_n;
    }
    d->ted_mu_q20 = mu;
}

/**
 * Greatest common divisor via Euclidean algorithm.
 */
static inline int gcd_int(int a, int b)
{
    if (a < 0) a = -a;
    if (b < 0) b = -b;
    while (b != 0) {
        int t = a % b;
        a = b;
        b = t;
    }
    return (a == 0) ? 1 : a;
}

/**
 * Normalized sinc function: sin(pi*x)/(pi*x), with sinc(0)=1.
 */
static inline double dsd_fme_sinc(double x)
{
    if (x == 0.0) return 1.0;
    return sin(kPi * x) / (kPi * x);
}

/**
 * Design windowed-sinc low-pass prototype for polyphase upfirdn (runs at L*Fs_in).
 * Uses a Hamming window and conservative cutoff to balance CPU vs. stopband.
 * Taps are stored phase-major with stride L (k*L + phase) so each phase sees
 * a contiguous sub-filter. Taps are normalized to give ~unity DC per phase,
 * then scaled by L to compensate polyphase upsampling (maintains amplitude).
 *
 * @param s Demodulator state to receive resampler taps/history.
 * @param L Upsampling factor.
 * @param M Downsampling factor.
 */
static void resamp_design(struct demod_state *s, int L, int M)
{
    /* Per-phase taps K; total taps = K*L. Keep small for CPU, but adequate stopband. */
    int taps_per_phase = 16; /* K */
    int total_taps = taps_per_phase * L;
    if (total_taps < L) total_taps = L;
    if (taps_per_phase < 8) taps_per_phase = 8;

    /* Normalized cutoff: conservative 0.45 * min(1/L, 1/M) of Nyquist at L*Fs. */
    double fc = 0.45 / (double)((L > M) ? L : M); /* 0..0.5 at L*Fs */
    int N = total_taps;
    int mid = (N - 1) / 2;

    /* Allocate taps if needed */
    if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; }
    if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; }
    s->resamp_taps = static_cast<int16_t*>(malloc((size_t)N * sizeof(int16_t)));
    s->resamp_hist = static_cast<int16_t*>(malloc((size_t)taps_per_phase * sizeof(int16_t)));
    if (!s->resamp_taps || !s->resamp_hist) {
        if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; }
        if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; }
        s->resamp_enabled = 0;
        fprintf(stderr, "Rational resampler: allocation failed, disabling.\n");
        return;
    }
    memset(s->resamp_hist, 0, (size_t)taps_per_phase * sizeof(int16_t));
    s->resamp_hist_head = 0;

    /* Windowed-sinc (Hamming) */
    double gain = 0.0;
    for (int n = 0; n < N; n++) {
        int m = n - mid;
        double w = 0.54 - 0.46 * cos(2.0 * kPi * (double)n / (double)(N - 1));
        double h = 2.0 * fc * dsd_fme_sinc(2.0 * fc * (double)m);
        double t = h * w;
        gain += t;
    }
    /* Normalize to unity DC gain */
    if (gain == 0.0) gain = 1.0;
    /* Compensate for polyphase upsampling: scale taps by L so that each phase
       has approximately unity DC gain (preserves amplitude through upfirdn). */
    const double phase_gain_comp = (double)L;
    for (int n = 0; n < N; n++) {
        int m = n - mid;
        double w = 0.54 - 0.46 * cos(2.0 * kPi * (double)n / (double)(N - 1));
        double h = 2.0 * fc * dsd_fme_sinc(2.0 * fc * (double)m);
        double t = (h * w / gain) * phase_gain_comp;
        int v = (int)lrint(t * (double)(1 << 15));
        if (v >  32767) v =  32767;
        if (v < -32768) v = -32768;
        s->resamp_taps[n] = (int16_t)v;
    }

    s->resamp_L = L;
    s->resamp_M = M;
    s->resamp_phase = 0;
    s->resamp_taps_len = N;
    s->resamp_taps_per_phase = taps_per_phase;
}

/**
 * Process one block using polyphase upfirdn with history.
 *
 * @param s      Demodulator state containing resampler state.
 * @param in     Pointer to input samples.
 * @param in_len Number of input samples.
 * @param out    Pointer to output buffer (sized to hold produced samples).
 * @return Number of output samples written.
 */
static int resamp_process_block(struct demod_state *s, const int16_t *in, int in_len, int16_t *out)
{
    if (!s->resamp_enabled || !s->resamp_taps || !s->resamp_hist) {
        /* passthrough */
        memcpy(out, in, (size_t)in_len * sizeof(int16_t));
        return in_len;
    }
    const int L = s->resamp_L;
    const int M = s->resamp_M;
    const int K = s->resamp_taps_per_phase; /* taps per phase */
    const int16_t *taps = s->resamp_taps;   /* length K*L, phase-major stride L */
    int phase = s->resamp_phase;            /* 0..L-1 */
    int head = s->resamp_hist_head;         /* circular head index */
    int out_len = 0;

    for (int n = 0; n < in_len; n++) {
        /* Push new sample into circular history at head */
        s->resamp_hist[head] = in[n];
        head++;
        if (head == K) head = 0;

        /* While we owe outputs with current input available */
        int local_phase = phase;
        while (local_phase < L) {
            /* Dot: y = sum_{k=0..K-1} hist[idx] * taps[k*L + local_phase]
               Access history most-recent-first starting from head-1. */
            int64_t acc = 0;
            const int16_t *tk = taps + local_phase;
            int idx = head - 1; if (idx < 0) idx += K;
            for (int k = 0; k < K; k++) {
                acc += (int32_t)s->resamp_hist[idx] * (int32_t)tk[0];
                tk += L;
                idx--; if (idx < 0) idx += K;
            }
            /* Q15 -> Q0 with rounding and saturation */
            acc += (1 << 14);
            int32_t y = (int32_t)(acc >> 15);
            out[out_len++] = sat16(y);
            local_phase += M;
        }
        phase = local_phase - L;
    }

    s->resamp_phase = phase;
    s->resamp_hist_head = head;
    return out_len;
}

/**
 * DC-corrected mean power (sqrt-free). Integer-only implementation.
 *
 * @param samples Input sample buffer.
 * @param len     Number of samples to process.
 * @param step    Step between processed samples (subsampling).
 * @return Mean power (squared RMS) with DC bias removed.
 */
long int mean_power(int16_t *samples, int len, int step)
{
	int64_t p = 0;
	int64_t t = 0;
	for (int i = 0; i < len; i += step) {
		int64_t s = (int64_t)samples[i];
		t += s;
		p += s * s;
	}
	/* DC-corrected energy ≈ p - (t^2)/len with rounded division */
	int64_t dc_corr = 0;
	if (len > 0) {
		int64_t tt = t * t;
		dc_corr = (tt + (len / 2)) / len;
	}
	int64_t energy = p - dc_corr;
	if (energy < 0) energy = 0;
	return (long int)(energy / (len > 0 ? len : 1));
}

/**
 * Full demodulation pipeline for one block.
 * Applies decimation (HB cascade or legacy), optional FLL and timing
 * correction, followed by the configured discriminator and post-processing.
 *
 * @param d Demodulator state (consumes lowpassed, produces result).
 */
void full_demod(struct demod_state *d)
{
	int i, ds_p;
	ds_p = d->downsample_passes;
	if (ds_p) {
		/* Choose decimator: half-band cascade (default) or legacy path */
		if (use_halfband_decimator) {
			/* Apply ds_p stages of 2:1 half-band decimation on interleaved lowpassed */
			int in_len = d->lp_len;
			int16_t *src = d->lowpassed;
			int16_t *dst = d->hb_workbuf;
			for (i = 0; i < ds_p; i++) {
				/* Fused complex HB decimation on interleaved I/Q */
				int out_len_interleaved = hb_decim2_complex_interleaved(src, in_len, dst, d->hb_hist_i[i], d->hb_hist_q[i]);
				/* Next stage */
				src = dst;
				in_len = out_len_interleaved;
				dst = (src == d->hb_workbuf) ? d->lowpassed : d->hb_workbuf;
			}
			/* Final output resides in 'src' with length in_len; consume in-place (no copy) */
			d->lowpassed = src;
			d->lp_len = in_len;
			/* No droop compensation for half-band cascade */
		} else {
			for (i=0; i < ds_p; i++) {
				fifth_order(d->lowpassed,   (d->lp_len >> i), d->lp_i_hist[i]);
				fifth_order(d->lowpassed+1, (d->lp_len >> i) - 1, d->lp_q_hist[i]);
			}
			d->lp_len = d->lp_len >> ds_p;
			/* droop compensation */
			if (d->comp_fir_size == 9 && ds_p <= CIC_TABLE_MAX) {
				generic_fir(d->lowpassed, d->lp_len,
					cic_9_tables[ds_p], d->droop_i_hist);
				generic_fir(d->lowpassed+1, d->lp_len-1,
					cic_9_tables[ds_p], d->droop_q_hist);
			}
		}
	} else {
		low_pass(d);
	}
	/* Residual CFO correction before discriminator */
	fll_mix_and_update(d);
	/* Lightweight timing error correction (optional, avoid for analog FM demod) */
	if (d->ted_enabled && (d->mode_demod != &fm_demod || d->ted_force)) {
		gardner_timing_adjust(d);
	}
	/* Power squelch (sqrt-free): compare mean power estimate against squared threshold.
	   Samples are decimated by `squelch_decim_stride`; an EMA smooths block power.
	   The sampling phase advances by lp_len % stride per block to cover all offsets. */
	if (d->squelch_level) {
		/* Decimated block power estimate (no DC correction; EMA smooths) */
		int stride = (d->squelch_decim_stride > 0) ? d->squelch_decim_stride : 16;
		int phase = d->squelch_decim_phase;
		int64_t p = 0;
		int count = 0;
		for (int j = phase; j < d->lp_len; j += stride) {
			int64_t s2 = (int64_t)d->lowpassed[j];
			p += s2 * s2;
			count++;
		}
		/* Advance phase to sample different positions next block */
		if (stride > 0) {
			int adv = d->lp_len % stride;
			d->squelch_decim_phase = (phase + adv) % stride;
		}
		if (count > 0) {
			int64_t block_mean = p / count;
			if (d->squelch_running_power == 0) {
				/* Initialize on first measurement to avoid long ramp */
				d->squelch_running_power = block_mean;
			} else {
				/* EMA: running += (block_mean - running) / window, window ~ 2^shift */
				int w = (d->squelch_window > 0) ? d->squelch_window : 2048;
				int shift = 0;
				/* approximate log2(window), prefer power-of-two windows */
				while ((1 << shift) < w && shift < 30) { shift++; }
				int64_t delta = (block_mean - d->squelch_running_power);
				d->squelch_running_power += (delta >> shift);
			}
		}
		int64_t thr2 = (int64_t)d->squelch_level * (int64_t)d->squelch_level;
		if (d->squelch_running_power < thr2) {
			d->squelch_hits++;
			for (i=0; i<d->lp_len; i++) {
				d->lowpassed[i] = 0;
			}
		} else {
			d->squelch_hits = 0;
		}
	}
	d->mode_demod(d);  /* lowpassed -> result */
	if (d->mode_demod == &raw_demod) {
		return;
	}
	/* todo, fm noise squelch */
	// use nicer filter here too?
	if (d->post_downsample > 1) {
		d->result_len = low_pass_simple(d->result, d->result_len, d->post_downsample);}
	if (d->deemph) {
		deemph_filter(d);} 
	/* Optional post-demod audio LPF */
	audio_lpf_filter(d);
	if (d->dc_block) {
		dc_block_filter(d);}
	if (d->rate_out2 > 0) {
		low_pass_real(d);
		//arbitrary_resample(d->result, d->result, d->result_len, d->result_len * d->rate_out2 / d->rate_out);
	}
}

/**
 * RTL-SDR asynchronous USB callback.
 * Converts incoming u8 I/Q to s16 and enqueues into the input ring. If
 * `offset_tuning` is off and `DSD_FME_COMBINE_ROT` is enabled (default), a
 * combined rotate+widen implementation is used. Otherwise it falls back to
 * legacy two-pass (rotate_90 u8, then widen subtracting 128) or a simple
 * widen subtracting 127. On overflow, drops oldest ring data to avoid stalls.
 *
 * @param buf USB I/Q byte buffer.
 * @param len Buffer length in bytes (I/Q interleaved).
 * @param ctx Opaque pointer to `dongle_state`.
 */
static void rtlsdr_callback(unsigned char *buf, uint32_t len, void *ctx)
{
	
	struct dongle_state *s = static_cast<dongle_state*>(ctx);
	/* One-time: ensure the USB callback thread gets RT scheduling/affinity if enabled */
	{
		static std::atomic<int> usb_sched_applied{0};
		int expected = 0;
		if (usb_sched_applied.compare_exchange_strong(expected, 1)) {
			maybe_set_thread_realtime_and_affinity("USB");
		}
	}

	if (exitflag) {
		return;}
	if (!ctx) {
		return;}
	if (s->mute) {
		/* Clamp mute length to buffer size to avoid overwrite; carry remainder */
		int old = s->mute.load(std::memory_order_relaxed);
		if (old > 0) {
			uint32_t m = (uint32_t)old;
			if (m > len) m = len;
			memset(buf, 127, m);
			s->mute.fetch_sub((int)m, std::memory_order_relaxed);
		}
	}
	/* Convert incoming u8 I/Q and write directly into input ring without extra copy */
	size_t need = len;
	size_t done = 0;
	/* For legacy two-pass path, rotate the incoming byte buffer once up front */
	int use_two_pass = (!s->offset_tuning && !combine_rotate_enabled);
	if (use_two_pass) {
		rotate_90(buf, len);
	}
	while (need > 0) {
		int16_t *p1 = NULL, *p2 = NULL; size_t n1 = 0, n2 = 0;
		input_ring_reserve(&input_ring, need, &p1, &n1, &p2, &n2);
		if (n1 == 0 && n2 == 0) {
			/* Ring still full after drop attempt; give up this callback to avoid stall */
			break;
		}
		/* Ensure even counts to keep I/Q pairs aligned */
		if (n1 & 1) n1--;
		size_t w1 = (n1 < need) ? n1 : need;
		size_t rem_after_w1 = need - w1;
		if (n2 & 1) n2--;
		size_t w2 = (n2 < rem_after_w1) ? n2 : rem_after_w1;

		if (!s->offset_tuning && combine_rotate_enabled) {
			if (w1) widen_rotate90_u8_to_s16_bias127(buf + done, p1, (uint32_t)w1);
			if (w2) widen_rotate90_u8_to_s16_bias127(buf + done + w1, p2, (uint32_t)w2);
		} else if (use_two_pass) {
			/* bytes already rotated in-place; widen with 128 subtraction to avoid bias */
			if (w1) widen_u8_to_s16_bias128_scalar(buf + done, p1, (uint32_t)w1);
			if (w2) widen_u8_to_s16_bias128_scalar(buf + done + w1, p2, (uint32_t)w2);
		} else {
			if (w1) widen_u8_to_s16_bias127(buf + done, p1, (uint32_t)w1);
			if (w2) widen_u8_to_s16_bias127(buf + done + w1, p2, (uint32_t)w2);
		}
		input_ring_commit(&input_ring, w1 + w2);
		done += w1 + w2;
		need -= w1 + w2;
	}
}

/**
 * RTL-SDR USB thread entry: reads samples asynchronously into the input ring.
 * Applies optional realtime scheduling/affinity if configured.
 *
 * @param arg Pointer to `dongle_state`.
 * @return NULL on exit.
 */
static void *dongle_thread_fn(void *arg)
{
	struct dongle_state *s = static_cast<dongle_state*>(arg);
	maybe_set_thread_realtime_and_affinity("DONGLE");
	rtlsdr_read_async(s->dev, rtlsdr_callback, s, 16, s->buf_len);
	return 0;
}

/**
 * Demodulation thread entry: reads from input ring, runs the demod pipeline,
 * and writes audio samples to the output ring.
 *
 * @param arg Pointer to `demod_state`.
 * @return NULL on exit.
 */
static void *demod_thread_fn(void *arg)
{
	struct demod_state *d = static_cast<demod_state*>(arg);
	struct output_state *o = d->output_target;
	maybe_set_thread_realtime_and_affinity("DEMOD");
	while (!exitflag) {
		/* Read a block from input ring */
		int got = input_ring_read_block(&input_ring, d->input_cb_buf, MAXIMUM_BUF_LENGTH);
		if (got <= 0) continue;
		d->lowpassed = d->input_cb_buf;
		d->lp_len = got;
		full_demod(d);
		if (d->exit_flag) {
			exitflag = 1;
		}
		if (d->squelch_level && d->squelch_hits > d->conseq_squelch) {
			d->squelch_hits = d->conseq_squelch + 1;  /* hair trigger */
			safe_cond_signal(&controller.hop, &controller.hop_m);
			continue;
		}
		/* Preferred path: rational resampler when enabled; otherwise legacy upsampler */
		if (d->resamp_enabled) {
			int out_n = resamp_process_block(d, d->result, d->result_len, d->resamp_outbuf);
			if (out_n > 0) ring_write_signal_on_empty_transition(o, d->resamp_outbuf, (size_t)out_n);
		} else {
			/* Legacy path: optional simple upsampler */
			if (bandwidth_multiplier <= 1) {
				ring_write_signal_on_empty_transition(o, d->result, (size_t)d->result_len);
			} else {
				const int M = bandwidth_multiplier;
				const int N = d->result_len;
				if (N <= 0) {
					/* nothing to write */
				} else if (N == 1) {
					for (int m = 0; m < M; m++) d->upsample_buf[m] = d->result[0];
					ring_write_signal_on_empty_transition(o, d->upsample_buf, (size_t)M);
				} else {
					const size_t up_len = (size_t)N * (size_t)M;
					struct UpArg { int start; int end; int M; const int16_t *src; int16_t *dst; };
					auto up_task = [](void *arg){
						UpArg *a = (UpArg*)arg;
						const int Mloc = a->M;
						for (int n = a->start; n < a->end; n++) {
							int32_t x0 = a->src[n];
							int32_t x1 = a->src[n + 1];
							int16_t *row = a->dst + (size_t)n * (size_t)Mloc;
							if (upsample_fixedpoint_enabled) {
								int32_t dx = x1 - x0;
								int64_t step_q15_64 = ((int64_t)dx << 15) / (int64_t)Mloc;
								int32_t step_q15 = (int32_t)step_q15_64;
								int32_t acc_q15 = 0;
								for (int m = 0; m < Mloc; m++) {
									int32_t frac = (acc_q15 >= 0) ? ((acc_q15 + (1 << 14)) >> 15) : -(((-acc_q15) + (1 << 14)) >> 15);
									int32_t interp = x0 + frac;
									row[m] = (int16_t)interp;
									acc_q15 += step_q15;
								}
							} else {
								int32_t dx = x1 - x0;
								for (int m = 0; m < Mloc; m++) {
									int32_t interp = x0 + (dx * m) / Mloc;
									row[m] = (int16_t)interp;
								}
							}
						}
					};
					int mid = (N - 1) / 2;
					UpArg a0 = {0, mid, M, d->result, d->upsample_buf};
					UpArg a1 = {mid, N - 1, M, d->result, d->upsample_buf};
					if (d->mt_enabled) {
						demod_mt_run_two(d, up_task, (void*)&a0, up_task, (void*)&a1);
					} else {
						up_task((void*)&a0);
						up_task((void*)&a1);
					}
					d->upsample_buf[(size_t)(N - 1) * (size_t)M] = d->result[N - 1];
					if (upsample_fixedpoint_enabled) {
						for (int t = 1; t < M; t++) {
							d->upsample_buf[(size_t)(N - 1) * (size_t)M + (size_t)t] = d->result[N - 1];
						}
					}
					ring_write_signal_on_empty_transition(o, d->upsample_buf, up_len);
				}
			}
		}
		/* Signaling occurs only when the ring transitions from empty to non-empty. */
	}
	return 0;
}

/**
 * Find the nearest supported tuner gain to the requested value.
 *
 * @param dev          RTL-SDR device handle.
 * @param target_gain  Desired gain in tenths of dB.
 * @return Nearest supported gain in tenths of dB, or a negative error code.
 */
int nearest_gain(rtlsdr_dev_t *dev, int target_gain)
{
	int i, r, err1, err2, count, nearest;
	int* gains;
	r = rtlsdr_set_tuner_gain_mode(dev, 1);
	if (r < 0) {
		fprintf (stderr, "WARNING: Failed to enable manual gain.\n");
		return r;
	}
	count = rtlsdr_get_tuner_gains(dev, NULL);
	if (count <= 0) {
		return 0;
	}
	gains = static_cast<int*>(malloc(sizeof(int) * count));
	count = rtlsdr_get_tuner_gains(dev, gains);
	nearest = gains[0];
	for (i=0; i<count; i++) {
		err1 = abs(target_gain - nearest);
		err2 = abs(target_gain - gains[i]);
		if (err2 < err1) {
			nearest = gains[i];
		}
	}
	free(gains);
	return nearest;
}

/**
 * Set RTL-SDR center frequency with a brief status message.
 *
 * @param dev       RTL-SDR device handle.
 * @param frequency Center frequency in Hz.
 * @return 0 on success or a negative error code.
 */
int verbose_set_frequency(rtlsdr_dev_t *dev, uint32_t frequency)
{
	int r;
	r = rtlsdr_set_center_freq(dev, frequency);
	if (r < 0) {
		fprintf (stderr, " (WARNING: Failed to set Center Frequency). \n");
	} else {
		fprintf (stderr, " (Center Frequency: %u Hz.) \n", frequency);
	}
	return r;
}

/**
 * Set RTL-SDR sampling rate with a brief status message.
 *
 * @param dev       RTL-SDR device handle.
 * @param samp_rate Sampling rate in Hz.
 * @return 0 on success or a negative error code.
 */
int verbose_set_sample_rate(rtlsdr_dev_t *dev, uint32_t samp_rate)
{
	int r;
	r = rtlsdr_set_sample_rate(dev, samp_rate);
	if (r < 0) {
		fprintf (stderr, "WARNING: Failed to set sample rate.\n");
	} else {
		fprintf (stderr, "Sampling at %u S/s.\n", samp_rate);
	}
	return r;
}

/**
 * Enable or disable direct sampling mode.
 *
 * @param dev RTL-SDR device handle.
 * @param on  Non-zero to enable, zero to disable.
 * @return 0 on success or a negative error code.
 */
int verbose_direct_sampling(rtlsdr_dev_t *dev, int on)
{
	int r;
	r = rtlsdr_set_direct_sampling(dev, on);
	if (r != 0) {
		fprintf (stderr, "WARNING: Failed to set direct sampling mode.\n");
		return r;
	}
	if (on == 0) {
		fprintf (stderr, "Direct sampling mode disabled.\n");}
	if (on == 1) {
		fprintf (stderr, "Enabled direct sampling mode, input 1/I.\n");}
	if (on == 2) {
		fprintf (stderr, "Enabled direct sampling mode, input 2/Q.\n");}
	return r;
}

/**
 * Enable offset tuning on the tuner if supported.
 *
 * @param dev RTL-SDR device handle.
 * @return 0 on success or a negative error code.
 */
int verbose_offset_tuning(rtlsdr_dev_t *dev)
{
	int r;
	r = rtlsdr_set_offset_tuning(dev, 1);
	if (r != 0) {
		fprintf (stderr, "WARNING: Failed to set offset tuning.\n");
	} else {
		fprintf (stderr, "Offset tuning mode enabled.\n");
	}
	return r;
}

/**
 * Enable tuner automatic gain control.
 *
 * @param dev RTL-SDR device handle.
 * @return 0 on success or a negative error code.
 */
int verbose_auto_gain(rtlsdr_dev_t *dev)
{
	int r;
	r = rtlsdr_set_tuner_gain_mode(dev, 0);
	if (r != 0) {
		fprintf (stderr, "WARNING: Failed to set tuner gain.\n");
	} else {
		fprintf (stderr, "Tuner gain set to automatic.\n");
	}
	return r;
}

/**
 * Set a fixed tuner gain with a message indicating the result.
 *
 * @param dev  RTL-SDR device handle.
 * @param gain Desired gain in tenths of dB.
 * @return 0 on success or a negative error code.
 */
int verbose_gain_set(rtlsdr_dev_t *dev, int gain)
{
	int r;
	r = rtlsdr_set_tuner_gain_mode(dev, 1);
	if (r < 0) {
		fprintf (stderr, "WARNING: Failed to enable manual gain.\n");
		return r;
	}
	r = rtlsdr_set_tuner_gain(dev, gain);
	if (r != 0) {
		fprintf (stderr, "WARNING: Failed to set tuner gain.\n");
	} else {
		fprintf (stderr, "Tuner gain set to %0.2f dB.\n", gain/10.0);
	}
	return r;
}

/**
 * Set tuner PPM frequency error correction.
 *
 * @param dev        RTL-SDR device handle.
 * @param ppm_error  Error in parts-per-million.
 * @return 0 on success or a negative error code.
 */
int verbose_ppm_set(rtlsdr_dev_t *dev, int ppm_error)
{
	int r;
	// if (ppm_error == 0) {
	// 	return 0;}
	r = rtlsdr_set_freq_correction(dev, ppm_error);
	if (r < 0) {
		fprintf (stderr, "WARNING: Failed to set ppm error.\n");
	} else {
		fprintf (stderr, "Tuner error set to %i ppm.\n", ppm_error);
	}
	return r;
}

/**
 * Reset RTL-SDR USB buffers.
 *
 * @param dev RTL-SDR device handle.
 * @return 0 on success or a negative error code.
 */
int verbose_reset_buffer(rtlsdr_dev_t *dev)
{
	int r;
	r = rtlsdr_reset_buffer(dev);
	if (r < 0) {
		fprintf (stderr, "WARNING: Failed to reset buffers.\n");}
	return r;
}

/**
 * Compute and stage tuner/demodulator capture settings based on the
 * requested center frequency and current demod configuration. The actual
 * device programming occurs elsewhere after these fields are updated.
 *
 * @param freq Desired RF center frequency in Hz.
 * @param rate Current input sample rate (unused).
 */
static void optimal_settings(int freq, int rate)
{
	UNUSED(rate);

	// giant ball of hacks
	// seems unable to do a single pass, 2:1
	int capture_freq, capture_rate;
	struct dongle_state *d = &dongle;
	struct demod_state *dm = &demod;
	struct controller_state *cs = &controller;
	dm->downsample = (1000000 / dm->rate_in) + 1; //dm->rate_in is the rtl_bandwidth value
	if (dm->downsample_passes) {
		dm->downsample_passes = (int)log2(dm->downsample) + 1;
		dm->downsample = 1 << dm->downsample_passes;
	}
	capture_freq = freq;
	capture_rate = dm->downsample * dm->rate_in; //
	if (!d->offset_tuning) {
		capture_freq = freq + capture_rate/4;} //
	capture_freq += cs->edge * dm->rate_in / 2;
	dm->output_scale = (1<<15) / (128 * dm->downsample);
	if (dm->output_scale < 1) {
		dm->output_scale = 1;}
	if (dm->mode_demod == &fm_demod) {
		dm->output_scale = 1;}
	d->freq = (uint32_t)capture_freq;
	d->rate = (uint32_t)capture_rate;
	// fprintf (stderr, "Capture Frequency: %i Rate: %i \n", capture_freq, capture_rate);
}

/**
 * Controller thread: handles basic scanning/hopping between channels.
 *
 * @param arg Pointer to `controller_state`.
 * @return NULL on exit.
 */
static void *controller_thread_fn(void *arg)
{
	// thoughts for multiple dongles
	// might be no good using a controller thread if retune/rate blocks
	int i;
	struct controller_state *s = static_cast<controller_state*>(arg);

	if (s->wb_mode) {
		for (i=0; i < s->freq_len; i++) {
			s->freqs[i] += 16000;}
	}

	/* set up primary channel */
	optimal_settings(s->freqs[0], demod.rate_in);
	if (dongle.direct_sampling) {
		verbose_direct_sampling(dongle.dev, 1);}
	if (dongle.offset_tuning) {
		verbose_offset_tuning(dongle.dev);}

	/* Set the frequency */
	verbose_set_frequency(dongle.dev, dongle.freq);
	fprintf (stderr, "Oversampling input by: %ix.\n", demod.downsample);
	fprintf (stderr, "Oversampling output by: %ix.\n", demod.post_downsample);
	fprintf (stderr, "Buffer size: %0.2fms\n",
		1000 * 0.5 * (float)ACTUAL_BUF_LENGTH / (float)dongle.rate);

	/* Set the sample rate */
	verbose_set_sample_rate(dongle.dev, dongle.rate);
	fprintf (stderr, "Output at %u Hz.\n", demod.rate_in/demod.post_downsample);

	while (!exitflag) {
		safe_cond_wait(&s->hop, &s->hop_m);
		if (s->freq_len <= 1) {
			continue;}
		/* hacky hopping */
		s->freq_now = (s->freq_now + 1) % s->freq_len;
		optimal_settings(s->freqs[s->freq_now], demod.rate_in);
		rtlsdr_set_center_freq(dongle.dev, dongle.freq);
		dongle.mute = BUFFER_DUMP;
	}
	return 0;
}

/**
 * Initialize dongle (RTL-SDR source) state with default parameters.
 *
 * @param s Dongle state to initialize.
 */
void dongle_init(struct dongle_state *s)
{
	s->rate = rtl_bandwidth;
	s->gain = AUTO_GAIN; // tenths of a dB
	s->mute = 0;
	s->direct_sampling = 0;
	s->offset_tuning = 0; //E4000 tuners only
	s->demod_target = &demod;
}

/**
 * Initialize demodulator state for analog FM path.
 *
 * @param s Demodulator state to initialize.
 */
void demod_init_analog(struct demod_state *s)
{
	s->rate_in = rtl_bandwidth;
	s->rate_out = rtl_bandwidth;
	s->squelch_level = 0;
	s->conseq_squelch = 10;
	s->terminate_on_squelch = 0;
	s->squelch_hits = 11;
	s->downsample_passes = 1; //
	s->comp_fir_size = 9;
	s->prev_index = 0;
	s->post_downsample = 1;  //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either
	s->custom_atan = 1;
	s->deemph = 1; //
	s->rate_out2 = rtl_bandwidth;  // -1 flag for disabled -- this enables low_pass_real, seems to work okay
	s->mode_demod = &fm_demod;
	s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
	s->prev_lpr_index = 0;
	s->deemph_a = 0; //
	s->deemph_avg = 0;
	/* Audio LPF defaults */
	s->audio_lpf_enable = 0;
	s->audio_lpf_alpha = 0;
	s->audio_lpf_state = 0;
	s->now_lpr = 0;
	s->dc_block = 1; //
	s->dc_avg = 0;
	/* Resampler defaults */
	s->resamp_enabled = 0;
	s->resamp_target_hz = 0;
	s->resamp_L = 1;
	s->resamp_M = 1;
	s->resamp_phase = 0;
	s->resamp_taps_len = 0;
	s->resamp_taps_per_phase = 0;
	s->resamp_taps = NULL;
	s->resamp_hist = NULL;
	/* FLL/TED defaults */
	s->fll_enabled = 0;
	s->fll_alpha_q15 = 0;
	s->fll_beta_q15 = 0;
	s->fll_freq_q15 = 0;
	s->fll_phase_q15 = 0;
	s->fll_prev_r = 0;
	s->fll_prev_j = 0;
	s->ted_enabled = 0;
	s->ted_gain_q20 = 0;
	s->ted_sps = 0;
	s->ted_mu_q20 = 0;
	/* Squelch estimator init */
	s->squelch_running_power = 0;
	s->squelch_decim_stride = 16; /* evaluate 1/16th samples for low CPU */
	s->squelch_decim_phase = 0;
	s->squelch_window = 2048; /* EMA window ~2048 samples */
	/* HB decimator histories */
	for (int st = 0; st < 10; st++) {
		memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st]));
		memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st]));
	}
	/* Legacy CIC histories used by fifth_order path */
	for (int st = 0; st < 10; st++) {
		memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st]));
		memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st]));
	}
	/* Input ring does not require double-buffer init */
	s->lowpassed = s->input_cb_buf;
	s->lp_len = 0;
	pthread_cond_init(&s->ready, NULL);
	pthread_mutex_init(&s->ready_m, NULL);
	s->output_target = &output;
	if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); }
	/* set discriminator function pointer */
	/* custom_atan mapping:
	   0 -> polar_discriminant (double atan2; slow, highest accuracy)
	   1 -> polar_disc_fast    (int64 fast_atan2 approximation)
	   2 -> polar_disc_lut     (LUT-based atan2 approximation)
	*/
	s->discriminator = (s->custom_atan == 0) ? &polar_discriminant :
		(s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut;
	/* Init minimal worker pool (env-gated) */
	demod_mt_init(s);
}

/**
 * Initialize demodulator state for RO2 path (no CIC, LUT atan by default).
 *
 * @param s Demodulator state to initialize.
 */
void demod_init_ro2(struct demod_state *s)
{
	s->rate_in = rtl_bandwidth;
	s->rate_out = rtl_bandwidth;
	s->squelch_level = 0;
	s->conseq_squelch = 10;
	s->terminate_on_squelch = 0;
	s->squelch_hits = 11;
	s->downsample_passes = 0;
	s->comp_fir_size = 0;
	s->prev_index = 0;
	s->post_downsample = 1;  //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either
	s->custom_atan = 2;
	s->deemph = 0;
	s->rate_out2 = rtl_bandwidth;  // -1 flag for disabled -- this enables low_pass_real, seems to work okay
	s->mode_demod = &fm_demod;
	s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
	s->prev_lpr_index = 0;
	s->deemph_a = 0;
	s->deemph_avg = 0;
	/* Audio LPF defaults */
	s->audio_lpf_enable = 0;
	s->audio_lpf_alpha = 0;
	s->audio_lpf_state = 0;
	s->now_lpr = 0;
	s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue
	s->dc_avg = 0;
	/* Resampler defaults */
	s->resamp_enabled = 0;
	s->resamp_target_hz = 0;
	s->resamp_L = 1;
	s->resamp_M = 1;
	s->resamp_phase = 0;
	s->resamp_taps_len = 0;
	s->resamp_taps_per_phase = 0;
	s->resamp_taps = NULL;
	s->resamp_hist = NULL;
	/* FLL/TED defaults */
	s->fll_enabled = 0;
	s->fll_alpha_q15 = 0;
	s->fll_beta_q15 = 0;
	s->fll_freq_q15 = 0;
	s->fll_phase_q15 = 0;
	s->fll_prev_r = 0;
	s->fll_prev_j = 0;
	s->ted_enabled = 0;
	s->ted_gain_q20 = 0;
	s->ted_sps = 0;
	s->ted_mu_q20 = 0;
	/* Squelch estimator init */
	s->squelch_running_power = 0;
	s->squelch_decim_stride = 16;
	s->squelch_decim_phase = 0;
	s->squelch_window = 2048;
	/* HB decimator histories */
	for (int st = 0; st < 10; st++) {
		memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st]));
		memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st]));
	}
	/* Legacy CIC histories used by fifth_order path */
	for (int st = 0; st < 10; st++) {
		memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st]));
		memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st]));
	}
	/* Input ring does not require double-buffer init */
	s->lowpassed = s->input_cb_buf;
	s->lp_len = 0;
	pthread_cond_init(&s->ready, NULL);
	pthread_mutex_init(&s->ready_m, NULL);
	s->output_target = &output;
	if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); }
	/* set discriminator function pointer */
	s->discriminator = (s->custom_atan == 0) ? &polar_discriminant :
		(s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut;
	/* Init minimal worker pool (env-gated) */
	demod_mt_init(s);
}

/**
 * Initialize demodulator state for default digital path.
 *
 * @param s Demodulator state to initialize.
 */
void demod_init(struct demod_state *s)
{
	s->rate_in = rtl_bandwidth;
	s->rate_out = rtl_bandwidth;
	s->squelch_level = 0;
	s->conseq_squelch = 10;
	s->terminate_on_squelch = 0;
	s->squelch_hits = 11;
	s->downsample_passes = 0;
	s->comp_fir_size = 0;
	s->prev_index = 0;
	s->post_downsample = 1;  //1 -- once this works, default = 4 -- doesn't work on the official rtl-sdr source code either
	s->custom_atan = 2;
	s->deemph = 0;
	s->rate_out2 = -1;  // -1 flag for disabled -- this enables low_pass_real, seems to work okay
	s->mode_demod = &fm_demod;
	s->pre_j = s->pre_r = s->now_r = s->now_j = 0;
	s->prev_lpr_index = 0;
	s->deemph_a = 0;
	s->deemph_avg = 0;
	/* Audio LPF defaults */
	s->audio_lpf_enable = 0;
	s->audio_lpf_alpha = 0;
	s->audio_lpf_state = 0;
	s->now_lpr = 0;
	s->dc_block = 1; //enabling by default, but offset tuning is also enabled, so center spike shouldn't be an issue
	s->dc_avg = 0;
	/* Resampler defaults */
	s->resamp_enabled = 0;
	s->resamp_target_hz = 0;
	s->resamp_L = 1;
	s->resamp_M = 1;
	s->resamp_phase = 0;
	s->resamp_taps_len = 0;
	s->resamp_taps_per_phase = 0;
	s->resamp_taps = NULL;
	s->resamp_hist = NULL;
	/* FLL/TED defaults */
	s->fll_enabled = 0;
	s->fll_alpha_q15 = 0;
	s->fll_beta_q15 = 0;
	s->fll_freq_q15 = 0;
	s->fll_phase_q15 = 0;
	s->fll_prev_r = 0;
	s->fll_prev_j = 0;
	s->ted_enabled = 0;
	s->ted_gain_q20 = 0;
	s->ted_sps = 0;
	s->ted_mu_q20 = 0;
	/* Squelch estimator init */
	s->squelch_running_power = 0;
	s->squelch_decim_stride = 16;
	s->squelch_decim_phase = 0;
	s->squelch_window = 2048;
	/* HB decimator histories */
	for (int st = 0; st < 10; st++) {
		memset(s->hb_hist_i[st], 0, sizeof(s->hb_hist_i[st]));
		memset(s->hb_hist_q[st], 0, sizeof(s->hb_hist_q[st]));
	}
	/* Legacy CIC histories used by fifth_order path */
	for (int st = 0; st < 10; st++) {
		memset(s->lp_i_hist[st], 0, sizeof(s->lp_i_hist[st]));
		memset(s->lp_q_hist[st], 0, sizeof(s->lp_q_hist[st]));
	}
	/* Input ring does not require double-buffer init */
	s->lowpassed = s->input_cb_buf;
	s->lp_len = 0;
	pthread_cond_init(&s->ready, NULL);
	pthread_mutex_init(&s->ready_m, NULL);
	s->output_target = &output;
	if (s->custom_atan == 2 && atan_lut == NULL) { atan_lut_init(); }
	/* set discriminator function pointer */
	s->discriminator = (s->custom_atan == 0) ? &polar_discriminant :
		(s->custom_atan == 1) ? &polar_disc_fast : &polar_disc_lut;
	/* Init minimal worker pool (env-gated) */
	demod_mt_init(s);
}

/**
 * Release resources owned by the demodulator state.
 *
 * @param s Demodulator state to clean up.
 */
void demod_cleanup(struct demod_state *s)
{
	pthread_cond_destroy(&s->ready);
	pthread_mutex_destroy(&s->ready_m);
	/* Destroy worker pool if enabled */
	demod_mt_destroy(s);
	/* Free resampler resources */
	if (s->resamp_taps) { free(s->resamp_taps); s->resamp_taps = NULL; }
	if (s->resamp_hist) { free(s->resamp_hist); s->resamp_hist = NULL; }
}

/**
 * Initialize output ring buffer and synchronization primitives.
 *
 * @param s Output state to initialize.
 */
void output_init(struct output_state *s)
{
	s->rate = rtl_bandwidth;
	pthread_cond_init(&s->ready, NULL);
	pthread_cond_init(&s->space, NULL);
	pthread_mutex_init(&s->ready_m, NULL);
	/* Allocate SPSC ring buffer */
	s->capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8);
	/* Try aligned allocation for better vectorized copies; fall back if unavailable */
	{
		void *mem_ptr = NULL;
#if defined(_POSIX_C_SOURCE) && (_POSIX_C_SOURCE >= 200112L)
		if (posix_memalign(&mem_ptr, DSD_FME_ALIGN, s->capacity * sizeof(int16_t)) != 0) {
			mem_ptr = malloc(s->capacity * sizeof(int16_t));
		}
#else
		mem_ptr = malloc(s->capacity * sizeof(int16_t));
#endif
		s->buffer = static_cast<int16_t*>(mem_ptr);
	}
	s->head.store(0);
	s->tail.store(0);
}

/**
 * Destroy output ring buffer and synchronization primitives.
 *
 * @param s Output state to clean up.
 */
void output_cleanup(struct output_state *s)
{
	pthread_cond_destroy(&s->ready);
	pthread_cond_destroy(&s->space);
	pthread_mutex_destroy(&s->ready_m);
	if (s->buffer) { free(s->buffer); s->buffer = NULL; }
}

/**
 * Initialize controller state (frequency list and hop control).
 *
 * @param s Controller state to initialize.
 */
void controller_init(struct controller_state *s)
{
	s->freqs[0] = 446000000;
	s->freq_len = 0;
	s->edge = 0;
	s->wb_mode = 0;
	pthread_cond_init(&s->hop, NULL);
	pthread_mutex_init(&s->hop_m, NULL);
}

/**
 * Destroy controller synchronization primitives.
 *
 * @param s Controller state to clean up.
 */
void controller_cleanup(struct controller_state *s)
{
	pthread_cond_destroy(&s->hop);
	pthread_mutex_destroy(&s->hop_m);
}

/**
 * Validate runtime options and controller state prior to starting streams.
 * Exits the process with an error message if constraints are violated.
 */
void sanity_checks(void)
{
	if (controller.freq_len == 0) {
		fprintf (stderr, "Please specify a frequency.\n");
		exit(1);
	}

	if (controller.freq_len >= FREQUENCIES_LIMIT) {
		fprintf (stderr, "Too many channels, maximum %i.\n", FREQUENCIES_LIMIT);
		exit(1);
	}

	if (controller.freq_len > 1 && demod.squelch_level == 0) {
		fprintf (stderr, "Please specify a squelch level.  Required for scanning multiple frequencies.\n");
		exit(1);
	}

}

/**
 * Convert a 5-byte UDP control message into an integer.
 * Expects the first byte to be a command and the next four bytes to represent
 * a little-endian 32-bit value.
 *
 * @param buf Pointer to 5-byte buffer.
 * @return Decoded 32-bit little-endian integer from bytes 1..4.
 */
static unsigned int chars_to_int(unsigned char* buf) {

	int i;
	unsigned int val = 0;

	for(i=1; i<5; i++) {
		val = val | ((buf[i]) << ((i-1)*8));
	}

	return val;
}

/**
 * UDP control thread: listens for frequency tuning commands and applies them.
 *
 * Message format: 5 bytes, where buf[0]==0 indicates a set-frequency command
 * and buf[1..4] is a little-endian 32-bit frequency in Hz.
 *
 * @param arg Unused.
 * @return NULL on exit.
 */
static void *socket_thread_fn(void *arg) {
  UNUSED(arg);

  int n;
  int sockfd;
  unsigned char buffer[5];
  struct sockaddr_in serv_addr;

	sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);

  if (sockfd < 0) {
  	perror("ERROR opening socket");
  }

	bzero((char *) &serv_addr, sizeof(serv_addr));

	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = INADDR_ANY;
	serv_addr.sin_port = htons(port);

	if (bind(sockfd, (struct sockaddr *) &serv_addr,  sizeof(serv_addr)) < 0) {
		perror("ERROR on binding");
	}

	bzero(buffer,5);

	fprintf (stderr, "Main socket started! :-) Tuning enabled on UDP/%d \n", port);

	int new_freq;

	while ((n = read(sockfd, buffer, 5)) > 0) {
		if (n == 5 && buffer[0] == 0) {
			new_freq = chars_to_int(buffer);
			dongle.freq = new_freq;
			optimal_settings(new_freq, demod.rate_in);
			rtlsdr_set_center_freq(dongle.dev, dongle.freq);
			fprintf (stderr, "\nTuning to: %d [Hz] \n", new_freq);
		}
	}
	if (n < 0) {
		perror("ERROR on read");
	}

	close(sockfd);
	return 0;
}

/**
 * Signal handler to request RTL-SDR async cancel and exit.
 */
void rtlsdr_sighandler(void)
{
	fprintf (stderr, "Signal caught, exiting!\n");
	rtlsdr_cancel_async(dongle.dev);
}

/**
 * Initialize and open the RTL-SDR streaming pipeline, threads, and buffers.
 *
 * @param opts Decoder options used to configure the pipeline.
 */
void open_rtlsdr_stream(dsd_opts *opts)
{
  int r;
	rtl_bandwidth =  opts->rtl_bandwidth * 1000; //reverted back to straight value
	bandwidth_multiplier = (bandwidth_divisor / rtl_bandwidth);
	/* Guard multiplier to a safe range [1, MAX_BANDWIDTH_MULTIPLIER] */
	{
		int orig_mult = bandwidth_multiplier;
		if (bandwidth_multiplier < 1) {
			fprintf(stderr,
				"WARNING: bandwidth_multiplier computed as %d (divisor=%d, bandwidth=%d Hz). Clamping to 1.\n",
				orig_mult, bandwidth_divisor, rtl_bandwidth);
			bandwidth_multiplier = 1;
		} else if (bandwidth_multiplier > MAX_BANDWIDTH_MULTIPLIER) {
			fprintf(stderr,
				"WARNING: bandwidth_multiplier computed as %d exceeds max %d (divisor=%d, bandwidth=%d Hz). Clamping to %d.\n",
				orig_mult, MAX_BANDWIDTH_MULTIPLIER, bandwidth_divisor, rtl_bandwidth, MAX_BANDWIDTH_MULTIPLIER);
			bandwidth_multiplier = MAX_BANDWIDTH_MULTIPLIER;
		}
	}
	volume_multiplier = 1; //moved to external handling to be more dynamic

	//this needs to be initted first, then we set the parameters
  dongle_init(&dongle);
	//init with low pass if decoding P25 or EDACS/Provoice
	if (opts->frame_p25p1 == 1 || opts->frame_p25p2 == 1 || opts->frame_provoice == 1)
  	demod_init_ro2(&demod);
	else if (opts->analog_only == 1 || opts->m17encoder == 1)
		demod_init_analog(&demod);
	else demod_init(&demod);
  output_init(&output);
  /* Init input ring */
  {
    void *mem_ptr = NULL;
#if defined(_POSIX_C_SOURCE) && (_POSIX_C_SOURCE >= 200112L)
    if (posix_memalign(&mem_ptr, DSD_FME_ALIGN, (size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t)) != 0) {
      mem_ptr = malloc((size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t));
    }
#else
    mem_ptr = malloc((size_t)(MAXIMUM_BUF_LENGTH * 8) * sizeof(int16_t));
#endif
    input_ring.buffer = static_cast<int16_t*>(mem_ptr);
    input_ring.capacity = (size_t)(MAXIMUM_BUF_LENGTH * 8);
    input_ring.head.store(0);
    input_ring.tail.store(0);
    pthread_cond_init(&input_ring.ready, NULL);
    pthread_mutex_init(&input_ring.ready_m, NULL);
  }
  controller_init(&controller);

	/* Read optional environment flags */
	{
		const char *hb = getenv("DSD_FME_HB_DECIM");
		if (hb && hb[0] != '\0') {
			int v = atoi(hb);
			use_halfband_decimator = (v != 0);
		}
		const char *cr = getenv("DSD_FME_COMBINE_ROT");
		if (cr && cr[0] != '\0') {
			combine_rotate_enabled = (atoi(cr) != 0);
		}
		const char *ufp = getenv("DSD_FME_UPSAMPLE_FP");
		if (ufp && ufp[0] != '\0') {
			upsample_fixedpoint_enabled = (atoi(ufp) != 0);
		}
		/* Configure rational resampler target rate via DSD_FME_RESAMP (Hz).
		   Defaults: enabled at 48000 Hz unless set to "off" or "0". */
		const char *rs = getenv("DSD_FME_RESAMP");
		int enable_resamp = 1;
		int target = 48000;
		if (rs && rs[0] != '\0') {
			if (strcasecmp(rs, "off") == 0 || strcmp(rs, "0") == 0) {
				enable_resamp = 0;
			} else {
				int v = atoi(rs);
				if (v > 0) target = v; else target = 48000;
			}
		}
		if (enable_resamp) {
			demod.resamp_target_hz = target;
			int inRate = (demod.rate_out > 0) ? demod.rate_out : rtl_bandwidth;
			int g = gcd_int(inRate, target);
			int L = target / g;
			int M = inRate / g;
			if (L < 1) L = 1;
			if (M < 1) M = 1;
			/* Guard output buffer growth (limited to ~4x expansion) */
			int scale_num = L;
			int scale_den = M;
			int scale = (scale_den > 0) ? ( (scale_num + scale_den - 1) / scale_den ) : 1;
			if (scale > 4) {
				fprintf(stderr, "Resampler ratio too large (L=%d,M=%d). Clamping not supported; disabling resampler.\n", L, M);
				demod.resamp_enabled = 0;
			} else {
				demod.resamp_enabled = 1;
				resamp_design(&demod, L, M);
				fprintf(stderr, "Rational resampler enabled: %d -> %d Hz (L=%d,M=%d).\n", inRate, target, L, M);
			}
		} else {
			demod.resamp_enabled = 0;
		}

		/* Configure FLL/TED via envs. Defaults: FLL on with small gains; TED off by default. */
		const char *fll = getenv("DSD_FME_FLL");
		demod.fll_enabled = (!fll || fll[0] == '\0' || fll[0] == '1') ? 1 : 0;
		/* Optional: enable LUT-based FLL rotator via DSD_FME_FLL_LUT=1 */
		{
			const char *flut = getenv("DSD_FME_FLL_LUT");
			fll_lut_enabled = (flut && flut[0] == '1') ? 1 : 0;
		}
		/* Gains in Q15; very conservative defaults */
		const char *fa = getenv("DSD_FME_FLL_ALPHA");
		const char *fb = getenv("DSD_FME_FLL_BETA");
		demod.fll_alpha_q15 = fa ? atoi(fa) : 100;  /* ~0.003 */
		demod.fll_beta_q15  = fb ? atoi(fb) : 10;   /* ~0.0003 */
		demod.fll_freq_q15  = 0;
		demod.fll_phase_q15 = 0;
		demod.fll_prev_r = demod.fll_prev_j = 0;

		const char *ted = getenv("DSD_FME_TED");
		demod.ted_enabled = (ted && ted[0] == '1') ? 1 : 0;
		const char *tg = getenv("DSD_FME_TED_GAIN");
		const char *ts = getenv("DSD_FME_TED_SPS");
		const char *tf = getenv("DSD_FME_TED_FORCE");
		demod.ted_gain_q20 = tg ? atoi(tg) : 64; /* tiny default */
		demod.ted_sps = ts ? atoi(ts) : 10;      /* e.g., 4800 sym/s @ 48k */
		demod.ted_mu_q20 = 0;
		demod.ted_force = (tf && tf[0] == '1') ? 1 : 0;

		/* Mode-aware defaults (only if envs not provided) */
		int env_ted_set = (ted && ted[0] != '\0');
		int env_fll_alpha_set = (fa && fa[0] != '\0');
		int env_fll_beta_set  = (fb && fb[0] != '\0');
		int env_ted_sps_set   = (ts && ts[0] != '\0');
		int env_ted_gain_set  = (tg && tg[0] != '\0');
		int digital_mode = (opts->frame_p25p1 == 1 || opts->frame_p25p2 == 1 || opts->frame_provoice == 1);
		/* Default: for common digital modes compute reasonable defaults, but keep TED disabled unless user opts in. */
		if (digital_mode) {
			if (!env_ted_set) demod.ted_enabled = 0;
			if (!env_ted_sps_set) {
				int Fs = (int)output.rate;
				int sps = (Fs + 2400) / 4800; /* round(Fs/4800) */
				if (sps < 2) sps = 2;
				demod.ted_sps = sps;
			}
			if (!env_ted_gain_set) {
				/* Slightly higher but safe default gain for digital */
				demod.ted_gain_q20 = 96;
			}
			if (!env_fll_alpha_set) demod.fll_alpha_q15 = 150; /* ~0.0046 */
			if (!env_fll_beta_set)  demod.fll_beta_q15  = 15;  /* ~0.00046 */
		} else {
			/* Analog defaults: keep TED off; gentle FLL */
			if (!env_ted_set) demod.ted_enabled = 0;
			if (!env_fll_alpha_set) demod.fll_alpha_q15 = 50; /* ~0.0015 */
			if (!env_fll_beta_set)  demod.fll_beta_q15  = 5;  /* ~0.00015 */
		}
	}

	if (opts->rtlsdr_center_freq > 0) {
		controller.freqs[controller.freq_len] = opts->rtlsdr_center_freq;
		controller.freq_len++;
	}

	if (opts->rtlsdr_ppm_error != 0) {
		dongle.ppm_error = opts->rtlsdr_ppm_error;
		fprintf (stderr, "Setting RTL PPM Error Set to %d\n", opts->rtlsdr_ppm_error);
	}

	dongle.dev_index = opts->rtl_dev_index;
	// demod.squelch_level = opts->rtl_squelch_level; //no longer used here, used in framesync vc pwr value under select conditions
	fprintf (stderr, "Setting RTL Bandwidth to %d Hz\n", rtl_bandwidth);
	// fprintf (stderr, "Setting RTL Sample Multiplier to %d\n", bandwidth_multiplier);
	fprintf (stderr, "Setting RTL Power Squelch Level to %d\n", opts->rtl_squelch_level);
	if (opts->rtl_udp_port != 0) port = opts->rtl_udp_port; //set this here, only open socket thread if set
	if (opts->rtl_gain_value > 0) {
		dongle.gain = opts->rtl_gain_value * 10; //multiple by ten to make it consitent with the way rtl_fm works
	}

  /* quadruple sample_rate to limit to Δθ to ±π/2 */
	demod.rate_in *= demod.post_downsample;

  if (!output.rate) output.rate = demod.rate_out;

  sanity_checks();

	if (controller.freq_len > 1) demod.terminate_on_squelch = 0;

  ACTUAL_BUF_LENGTH = lcm_post[demod.post_downsample] * DEFAULT_BUF_LENGTH;
  /* Ensure async read uses a valid, explicit buffer length */
  dongle.buf_len = (uint32_t)ACTUAL_BUF_LENGTH;

  r = rtlsdr_open(&dongle.dev, (uint32_t)dongle.dev_index);
  if (r < 0)
  {
    fprintf (stderr, "Failed to open rtlsdr device %d.\n", dongle.dev_index);
    exit(1);
  } else {
		fprintf (stderr, "Using RTLSDR Device Index: %d. \n", dongle.dev_index);
	}

  if (demod.deemph) {
		/* Configure deemphasis via env DSD_FME_DEEMPH: 75 (default), 50, nfm, off.
		   Computes a one-pole IIR with alpha = 1 - exp(-1/(Fs*tau)) stored in Q15. */
		double tau_s = 75e-6; /* default 75 microseconds */
		const char *deemph_env = getenv("DSD_FME_DEEMPH");
		if (deemph_env && deemph_env[0] != '\0') {
			if (strcasecmp(deemph_env, "off") == 0) {
				demod.deemph = 0;
			} else if (strcmp(deemph_env, "50") == 0) {
				tau_s = 50e-6;
			} else if (strcasecmp(deemph_env, "nfm") == 0) {
				/* Common NFM value */
				tau_s = 750e-6;
			} else if (strcmp(deemph_env, "75") == 0) {
				tau_s = 75e-6;
			}
		}
		if (demod.deemph) {
			/* a = exp(-1/(Fs*tau)); store alpha=(1-a) in Q15 */
			double Fs = (double)demod.rate_out;
			if (Fs < 1.0) Fs = 1.0;
			double a = exp(-1.0 / (Fs * tau_s));
			double alpha = 1.0 - a;
			int coef_q15 = (int)lrint(alpha * (double)(1 << 15));
			if (coef_q15 < 1) coef_q15 = 1; /* ensure non-zero to move toward steady-state */
			if (coef_q15 > (1 << 15)) coef_q15 = (1 << 15);
			demod.deemph_a = coef_q15;
		}
	}

	/* Configure optional post-demod audio LPF via env DSD_FME_AUDIO_LPF.
	   Values:
	   - off or 0: disabled (default)
	   - NNNN: cutoff in Hz (approximate), e.g., 3000 or 5000.
	   One-pole: y[n] = y[n-1] + alpha * (x[n] - y[n-1]),
	   alpha ≈ 1 - exp(-2*pi*fc/Fs) in Q15. */
	{
		const char *alpf = getenv("DSD_FME_AUDIO_LPF");
		demod.audio_lpf_enable = 0;
		demod.audio_lpf_alpha = 0;
		demod.audio_lpf_state = 0;
		if (alpf && alpf[0] != '\0') {
			if (strcasecmp(alpf, "off") == 0 || strcmp(alpf, "0") == 0) {
				/* disabled */
			} else {
				int cutoff_hz = atoi(alpf);
				if (cutoff_hz < 100) cutoff_hz = 100; /* guard */
				/* One-pole mapping: choose alpha from cutoff and Fs using approx alpha = 1 - exp(-2*pi*fc/Fs) */
				double Fs = (double)demod.rate_out;
				if (Fs < 1.0) Fs = 1.0;
				double a = 1.0 - exp(-2.0 * kPi * (double)cutoff_hz / Fs);
				if (a < 0.0) a = 0.0;
				if (a > 1.0) a = 1.0;
				int alpha_q15 = (int)lrint(a * (double)(1 << 15));
				if (alpha_q15 < 1) alpha_q15 = 1;
				if (alpha_q15 > (1 << 15)) alpha_q15 = (1 << 15);
				demod.audio_lpf_alpha = alpha_q15;
				demod.audio_lpf_enable = 1;
				fprintf(stderr, "Audio LPF enabled: fc≈%d Hz, alpha_q15=%d\n", cutoff_hz, demod.audio_lpf_alpha);
			}
		}
	}

  /* Set the tuner gain */
	if (dongle.gain == AUTO_GAIN) {
		verbose_auto_gain(dongle.dev);
		fprintf (stderr, "Setting RTL Autogain. \n");
	} else {
		dongle.gain = nearest_gain(dongle.dev, dongle.gain);
		verbose_gain_set(dongle.dev, dongle.gain);
		// fprintf (stderr, "Setting RTL Nearest Gain to %d. \n", dongle.gain); //seems to be working now
	}

  verbose_ppm_set(dongle.dev, dongle.ppm_error);

  /* Reset endpoint before we start reading from it (mandatory) */
	verbose_reset_buffer(dongle.dev);

  pthread_create(&controller.thread, NULL, controller_thread_fn, (void*)(&controller));
  usleep(100000);
  pthread_create(&demod.thread, NULL, demod_thread_fn, (void*)(&demod));
  pthread_create(&dongle.thread, NULL, dongle_thread_fn, (void*)(&dongle));
	//only create socket thread IF user specified (for legacy uses), else don't use it
	if (port != 0) pthread_create(&socket_freq, NULL, socket_thread_fn, (void *)(&controller));

	/* If resampler is enabled, update output.rate for downstream consumers */
	if (demod.resamp_enabled && demod.resamp_target_hz > 0) {
		output.rate = demod.resamp_target_hz;
		fprintf(stderr, "Output rate set to %d Hz via resampler.\n", output.rate);
	} else {
		output.rate = demod.rate_out;
	}
}

/**
 * Stop threads, cleanup buffers/objects, and close the RTL-SDR stream.
 */
void cleanup_rtlsdr_stream(void)
{
	fprintf (stderr, "cleaning up...\n");
  rtlsdr_cancel_async(dongle.dev);
  pthread_join(dongle.thread, NULL);
  safe_cond_signal(&demod.ready, &demod.ready_m);
  pthread_join(demod.thread, NULL);
  safe_cond_signal(&output.ready, &output.ready_m);
  safe_cond_signal(&controller.hop, &controller.hop_m);
  pthread_join(controller.thread, NULL);

  //dongle_cleanup(&dongle);
  demod_cleanup(&demod);
  output_cleanup(&output);
  controller_cleanup(&controller);

	/* free input ring */
	if (input_ring.buffer) { free(input_ring.buffer); input_ring.buffer = NULL; }

	/* free LUT memory if allocated */
	atan_lut_free();

  rtlsdr_close(dongle.dev);
}

/**
 * Batched consumer API: read up to count samples with fewer wakeups/locks.
 * Applies volume scaling.
 *
 * @param out   Destination buffer for audio samples.
 * @param count Maximum number of samples to read.
 * @param opts  Decoder options (used for runtime PPM changes).
 * @param state Decoder state (unused).
 * @return Number of samples read (>=1) or -1 on exit.
 */
int get_rtlsdr_samples(int16_t *out, size_t count, dsd_opts * opts, dsd_state * state)
{
	UNUSED(state);
	if (count == 0) return 0;

	/* If PPM Error is Manually Changed, change it here once per batch */
	if (opts->rtlsdr_ppm_error != dongle.ppm_error)
	{
		dongle.ppm_error = opts->rtlsdr_ppm_error;
		verbose_ppm_set(dongle.dev, dongle.ppm_error);
	}

	int got = ring_read_batch(&output, out, count);
	if (got <= 0) {
		return -1;
	}
	/* Apply volume scaling with saturation */
	for (int i = 0; i < got; i++) {
		int32_t y = (int32_t)out[i] * (int32_t)volume_multiplier;
		out[i] = sat16(y);
	}
	return got;
}

/**
 * Convenience wrapper to read a single sample via the batched API.
 *
 * @param sample Destination for one sample.
 * @param opts   Decoder options.
 * @param state  Decoder state (unused).
 * @return 0 on success, -1 on exit.
 */
int get_rtlsdr_sample(int16_t *sample, dsd_opts * opts, dsd_state * state)
{
	/* Delegate to batched API for a single sample */
	int ret = get_rtlsdr_samples(sample, 1, opts, state);
	if (ret < 0) return -1;
	return 0;
}

/**
 * Tune RTL-SDR to a new center frequency, updating optimal settings.
 *
 * @param opts      Decoder options.
 * @param frequency Target center frequency in Hz.
 */
void rtl_dev_tune(dsd_opts * opts, long int frequency)
{
	int r;
	if (opts->payload == 1)
		fprintf (stderr, "\nTuning to %lu Hz.", frequency);
	dongle.freq = opts->rtlsdr_center_freq = frequency;
	optimal_settings(dongle.freq, demod.rate_in);
	if (opts->payload == 1)
		fprintf (stderr, " (Center Frequency: %u Hz.) \n", dongle.freq);
	r = rtlsdr_set_center_freq(dongle.dev, dongle.freq);
	if (r < 0)
		fprintf (stderr, " (WARNING: Failed to set Center Frequency %u). \n", dongle.freq);

	rtl_clean_queue();

}

/**
 * Return mean power approximation (RMS^2 proxy) for soft squelch decisions.
 * Uses a small fixed sample window for efficiency.
 *
 * @return Mean power value (approximate RMS squared).
 */
long int rtl_return_pwr(void)
{
	long int pwr = 0;
	int n = demod.lp_len;
	if (n > 160) n = 160;
	if (n < 0) n = 0;
	pwr = mean_power(demod.lowpassed, n, 1);
	return (pwr);
}

/**
 * Clear the output ring buffer and wake any waiting producer.
 */
void rtl_clean_queue(void)
{
	/* Clear the entire ring to prevent sample 'lag' */
	ring_clear(&output);
	/* Wake producer waiting for space */
	safe_cond_signal(&output.space, &output.ready_m);
}