Nim Goerzel filters

goertzelfiltergeneral.nim

import strutils
import math
import complex

const 
  SampleRate = 8000.0  # 8kHz
  TargetFrequency = 941.0  # 941 Hz
  FFTSize = 205
  N = FFTSize  # Block size

type
  Sample = uint8
  
  # Generalized Goertzel filter
  GeneralizedGoertzelFilter = object
    frequency: float
    omega: float                                   # 2*pi*frequencyIndex/N
    coeff: float                                   # 2*cos(omega)
    complexConstant: Complex64                     # exp(-i*omega)
    s0, s1, s2: float                              # State variables
    sampleCount: int                               # number of processed samples


proc initGeneralizedGoertzel(frequency: float): GeneralizedGoertzelFilter =
  let 
    floatN = float(N)
    frequencyIndex = frequency * floatN / SampleRate  # Can be non-integer
    omega = 2.0 * Pi * frequencyIndex / floatN
  
  return GeneralizedGoertzelFilter(
    frequency: frequency,
    omega: omega,
    coeff: 2.0 * cos(omega),
    complexConstant: complex64(cos(-omega), sin(-omega)),  # exp(-i*omega)
    s0: 0.0,
    s1: 0.0,
    s2: 0.0,
    sampleCount: 0
  )


proc resetGeneralizedGoertzel(gf: var GeneralizedGoertzelFilter) =
  ## Reset the filter before processing a new block
  gf.s0 = 0.0
  gf.s1 = 0.0
  gf.s2 = 0.0
  gf.sampleCount = 0


proc processSampleGeneralized(gf: var GeneralizedGoertzelFilter, sample: Sample) =
  gf.sampleCount += 1  
  # For all but the last sample, update state variables
  if gf.sampleCount < N:
    gf.s0 = float(sample) + gf.coeff * gf.s1 - gf.s2
    gf.s2 = gf.s1
    gf.s1 = gf.s0
  else:
    # Process the last sample differently
    gf.s0 = float(sample) + gf.coeff * gf.s1 - gf.s2


proc getComplexResult(gf: GeneralizedGoertzelFilter): Complex64 =
  # Direct calculation from the state variables
  result = complex64(gf.s0, 0.0) - complex64(gf.s1 * gf.complexConstant.re, gf.s1 * gf.complexConstant.im)
  # Phase correction for non-integer frequency indices
  let phaseCorrection = complex64(
    cos(-gf.omega * float(N-1)), 
    sin(-gf.omega * float(N-1))
  )
  result = complex64(
    result.re * phaseCorrection.re - result.im * phaseCorrection.im,
    result.re * phaseCorrection.im + result.im * phaseCorrection.re
  )
  return result


proc processBlockGeneralized(gf: var GeneralizedGoertzelFilter, samples: openarray[Sample]): Complex64 =
  gf.resetGeneralizedGoertzel()
  for i in 0..<(samples.len-1):
    gf.sampleCount += 1
    gf.s0 = float(samples[i]) + gf.coeff * gf.s1 - gf.s2
    gf.s2 = gf.s1
    gf.s1 = gf.s0
  # Process the last sample
  gf.sampleCount += 1
  gf.s0 = float(samples[samples.len-1]) + gf.coeff * gf.s1 - gf.s2
  return gf.getComplexResult()


# Process multiple frequencies "at once"
proc goertzelGeneral(samples: openarray[Sample], frequencies: openarray[float]): seq[Complex64] =
  result = newSeq[Complex64](frequencies.len)
  for i, freq in frequencies:
    var gf = initGeneralizedGoertzel(freq)
    result[i] = gf.processBlockGeneralized(samples)


#=======
proc generate(testData: var openarray[Sample], frequency: float) =
  let step = frequency * ((2.0 * Pi) / SampleRate)  
  for index in 0..<N:
    testData[index] = Sample(100.0 * sin(float(index) * step) + 100.0)


proc demoGeneralizedGoertzel() =
  echo "Test Generalized Goertzel algorithm:"
  
  # Create test frequencies with non-integer indices
  let testFreqs = [
    TargetFrequency - 250.5,  # Non-integer offset
    TargetFrequency,
    TargetFrequency + 250.5   # Non-integer offset
  ]
  
  var testData: array[N, Sample]
  
  # Test each frequency
  for freq in testFreqs:
    echo "Test frequency: ", freq
    
    # Generate test data
    testData.generate(freq)
    
    # Process with generalized Goertzel
    var gf = initGeneralizedGoertzel(freq)
    let complexResult = gf.processBlockGeneralized(testData)
    
    # Calculate magnitude
    let 
      magnitude = sqrt(complexResult.re * complexResult.re + complexResult.im * complexResult.im)
      phase = arctan2(complexResult.im, complexResult.re)
    
    echo "  Complex result: re=", complexResult.re, " im=", complexResult.im
    echo "  Magnitude: ", magnitude
    echo "  Phase: ", phase, " radians"
    echo ""


# Demo to sweep through a range of frequencies including non-integer indices
proc frequencySweepDemo() =
  echo "Frequency Sweep with Generalized Goertzel:"
  
  var testData: array[N, Sample]
  
  # Create a range of frequencies to test
  var frequencies = newSeq[float]()
  var freq = TargetFrequency - 300.0
  while freq <= TargetFrequency + 300.0:
    frequencies.add(freq)
    freq += 15.25  # Non-integer step
  
  # Generate test data at the target frequency
  testData.generate(TargetFrequency)
  
  # Process all frequencies at once
  let results = goertzelGeneral(testData, frequencies)
  
  # Display results
  for i in 0..<frequencies.len:
    let 
      magnitude = sqrt(results[i].re * results[i].re + results[i].im * results[i].im)
      phase = arctan2(results[i].im, results[i].re)
    
    stdout.write "Freq=" & align($frequencies[i], 7, ' ') & "   "
    stdout.write "Mag=" & align($magnitude, 12, ' ') & "   "
    echo "Phase=" & align($phase, 8, ' ')



# Call this function to run the original demos and generalized Goertzel demos
proc runGoertzelDemos() =
  # Generalized Goertzel demos
  demoGeneralizedGoertzel()
  frequencySweepDemo()


proc main() =
  runGoertzelDemos()


main()

goertzelfilterjco.nim

import math, complex


proc gcd(a, b: int): int =
  ## Greatest common divisor using Euclidean algorithm
  var x = abs(a)
  var y = abs(b)
  while y != 0:
    (x, y) = (y, x mod y)
  return x


proc totient(n: var int): int =
  ## Euler's totient function φ(n)
  if n == 1:
    return 1
  
  result = n
  var i = 2
  while i * i <= n:
    if n mod i == 0:
      while n mod i == 0:
        n = n div i
      result = result - (result div i)
    i += 1
  
  if n > 1:
    result = result - (result div n)


proc cyclotomicPolynomial(L: var int): seq[int] =
  ## Generate coefficients of the L-th cyclotomic polynomial
  ## Simplified for the most common cases
  
  case L
  of 1: return @[1, -1]
  of 2: return @[1, 1]
  of 3: return @[1, 1, 1]
  of 4: return @[1, 0, 1]
  of 5: return @[1, 1, 1, 1, 1]
  of 6: return @[1, -1, 1]
  of 7: return @[1, 1, 1, 1, 1, 1, 1]
  of 8: return @[1, 0, 0, 0, 1]
  of 9: return @[1, 0, 0, 1, 0, 0, 1]
  of 10: return @[1, -1, 1, -1, 1]
  of 12: return @[1, 0, 1, 0, 1]
  of 15: return @[1, 1, 0, 0, 0, 0, 1, 1]
  of 20: return @[1, 0, 0, 0, 1, 0, 0, 0, 1]
  of 24: return @[1, 0, 0, 0, 0, 0, 0, 0, 1]
  of 30: return @[1, -1, 0, -1, 0, 1, 0, 1, 0, -1, 0, -1, 1]
  else:
    # For larger L, create a simple but consistent polynomial
    let phiL = totient(L)
    result = newSeq[int](phiL + 1)
    result[0] = 1
    result[phiL] = 1
    
    # Fill in some middle coefficients with typical pattern
    for i in 1..<phiL:
      if i mod 3 == 0:
        result[i] = if i mod 2 == 0: 1 else: -1
    
    return result


proc jcoGoertzelAlgorithm*(samples: seq[float], k: int, N: int): Complex64 =
  ## JCO-Goertzel algorithm
  ## Parameters:
  ##   samples - input signal samples
  ##   k - frequency bin index
  ##   N - number of samples (should match samples.len)
  ## Returns:
  ##   The complex DFT coefficient Vk
  
  # Ensure N matches the sample length
  assert(N == samples.len, "N must match the length of samples")
  
  # Handle special case for k = 0 (DC component)
  if k == 0:
    var sum = 0.0
    for s in samples:
      sum += s
    return complex(sum / N.float, 0.0)
  
  # Compute L = ord(W^k_N) = N / gcd(N, k)
  let gcdNk = gcd(N, k)
  var L = N div gcdNk
  let phiL = totient(L)
  
  echo "For k=", k, ", L=", L, ", φ(L)=", phiL
  
  # Special case handling for L=1 (which means W^k_N = 1)
  if L == 1:
    # Direct calculation using standard DFT definition
    result = complex(0.0, 0.0)
    let omega = 2.0 * PI * k.float / N.float
    
    for n in 0..<N:
      let angle = omega * n.float
      result += samples[n] * complex(cos(angle), -sin(angle))
    
    return result / N.float
  
  # Get the L-th cyclotomic polynomial coefficients
  let cycloPoly = cyclotomicPolynomial(L)
  
  # With low φ(L), use the standard Goertzel algorithm
  if phiL <= 2:
    if k == N div 4:  # Special case: cos(2πk/N) = 0, sin(2πk/N) = 1
      var q1, q2: float = 0.0
      for n in 0..<N:
        let q0 = samples[n] - q2
        q2 = q1
        q1 = q0
      return complex(q1, -q2) / 2.0
    elif k == N div 2:  # Special case: cos(2πk/N) = -1, sin(2πk/N) = 0
      var q1, q2: float = 0.0
      for n in 0..<N:
        let q0 = samples[n] + q1
        q2 = q1
        q1 = q0
      return complex(q1, -q2) / 2.0
    else:
      let omega = 2.0 * PI * k.float / N.float
      let cosine = cos(omega)
      let sine = sin(omega)
      let coeff = 2.0 * cosine
      
      var q1, q2: float = 0.0
      for n in 0..<N:
        let q0 = coeff * q1 - q2 + samples[n]
        q2 = q1
        q1 = q0
      
      let real = q1 - q2 * cosine
      let imag = q2 * sine
      
      return complex(real, -imag) / N.float
  
  # For larger φ(L), use JCO algorithm
  var state = newSeq[float](phiL)
  
  # Filter using the cyclotomic polynomial
  for n in 0..<N:
    for i in countdown(phiL-1, 1):
      state[i] = state[i-1]
    
    state[0] = samples[n]
    
    # Filter with coefficients from cyclotomic polynomial
    for i in 1..<cycloPoly.len:
      if cycloPoly[i] != 0:
        state[0] -= cycloPoly[i].float * state[i]
  
  # Polynomial at W^k_N
  result = complex(0.0, 0.0)
  let omega = 2.0 * PI * k.float / N.float
  
  for i in 0..<state.len:
    let angle = omega * i.float
    result += state[i] * complex(cos(angle), -sin(angle))
  
  # Scale the result
  result = result / N.float.sqrt()
  
  return result


proc jcoGoertzelSpectrum*(samples: seq[float], frequencies: seq[int]): seq[Complex64] =
  ## Compute the DFT coefficients for multiple frequency bins using JCO-Goertzel
  let N = samples.len
  result = newSeq[Complex64](frequencies.len)
  
  for i, k in frequencies:
    result[i] = jcoGoertzelAlgorithm(samples, k, N)


when isMainModule:
  # Test signal: a sum of 2 sine waves at 440Hz and 1000Hz, sampled at 8000Hz
  let sampleRate = 8000.0
  let freq1 = 440.0
  let freq2 = 1000.0
  let duration = 1.0  # seconds
  let N = int(sampleRate * duration)
  
  var samples = newSeq[float](N)
  for i in 0..<N:
    let t = i.float / sampleRate
    samples[i] = 0.5 * sin(2.0 * PI * freq1 * t) + 0.3 * sin(2.0 * PI * freq2 * t)
  
  # Bin indices for the frequencies of interest
  let bin1 = int(round(freq1 * N.float / sampleRate))
  let bin2 = int(round(freq2 * N.float / sampleRate))
  
  let frequencies = @[bin1, bin2]
  let spectrum = jcoGoertzelSpectrum(samples, frequencies)
  
  echo "Frequency components:"
  for i, k in frequencies:
    let magnitude = abs(spectrum[i])
    let phase = arctan2(spectrum[i].im, spectrum[i].re)
    echo "Bin ", k, " (", k.float * sampleRate / N.float, " Hz): magnitude = ", magnitude, ", phase = ", phase
  
  # Compute and show the expected magnitudes
  echo "\nExpected magnitudes:"
  echo "440 Hz: ", 0.5 / 2.0  # Half of the amplitude due to DFT properties
  echo "1000 Hz: ", 0.3 / 2.0

goertzelhoppinfilterbank.nim

import std/[math, random, sequtils, strformat, strutils]

const 
  SampleRate = 44100.0       # Hz
  N = 1024                   # Block size
  FFTSize = N
  MinFreq = 50.0             # Minimum frequency (Hz)
  MaxFreq = 15000.0          # Maximum frequency (Hz)

type
  Sample = uint16
  
  GoertzelFilter = object
    frequency: float
    coeff: float
    sine, cosine: float
    q1, q2: float
    gain: float

  GoertzelFilterBank = object
    fftSize: int
    octaves: int
    nFiltersOct: int
    minFreq: float
    maxFreq: float
    fBank: seq[GoertzelFilter]



proc initGoertzelFilter(frequency: float, fftSize: int): GoertzelFilter =
  let 
    floatN = float(fftSize)
    k = frequency * floatN / SampleRate
    omega = (2.0 * Pi * k) / floatN  
  result = GoertzelFilter(
    frequency: frequency,
    sine: sin(omega),
    cosine: cos(omega),
    q1: 0.0,
    q2: 0.0,
    gain: 1.0  # Default gain
  )
  result.coeff = 2.0 * result.cosine


proc createHoppingFilter(prev: GoertzelFilter, frequency: float, fftSize:int): GoertzelFilter =
  let 
    floatN = float(fftSize)
    prev_k = prev.frequency * floatN / SampleRate
    new_k = frequency * floatN / SampleRate
    delta_k = new_k - prev_k
    delta_omega = (2.0 * Pi * delta_k) / floatN
  result = GoertzelFilter(
    frequency: frequency,
    q1: 0.0,
    q2: 0.0,
    gain: 1.0  # Default gain
  )
  # New sine and cosine using the "hopping method"
  result.cosine = prev.cosine * cos(delta_omega) - prev.sine * sin(delta_omega)
  result.sine = prev.sine * cos(delta_omega) + prev.cosine * sin(delta_omega)
  result.coeff = 2.0 * result.cosine


proc initFilterBank(minFreq: float, octaves: int, nFiltersOct: int, fftSize: int): GoertzelFilterBank=
  let numFilters = octaves * nFiltersOct
  let maxFreq = minFreq.float * pow(2.0, octaves.float)
  result.fftSize = fftSize
  result.fbank = newSeq[GoertzelFilter](numFilters)  
  # Calculate logarithmically spaced frequencies
  for i in 0..<numFilters:
    let t = float(i) / float(numFilters - 1)
    let freq = minFreq * pow(maxFreq / minFreq, t)    
    if i == 0:
      result.fbank[i] = initGoertzelFilter(freq, fftSize)
    else:
      result.fbank[i] = createHoppingFilter(result.fbank[i-1], freq, fftSize)


proc resetFilterbank(filterbank: var GoertzelFilterBank) =
  for i in 0..<filterbank.fbank.len:
    filterbank.fbank[i].q1 = 0.0
    filterbank.fbank[i].q2 = 0.0


# Process a single sample through all filters, applying gains
proc processAllFilters(filterbank: var GoertzelFilterBank, sample: Sample) =
  let sampleValue = float(sample)  
  for i in 0..<filterbank.fbank.len:
    # Apply gain from image to the input sample
    let gainedSample = sampleValue * filterbank.fbank[i].gain    
    let q0 = filterbank.fbank[i].coeff * filterbank.fbank[i].q1 - filterbank.fbank[i].q2 + gainedSample
    filterbank.fbank[i].q2 = filterbank.fbank[i].q1
    filterbank.fbank[i].q1 = q0


# Calculate magnitudes for all filters
proc getAllMagnitudes(filterbank: GoertzelFilterBank): seq[float] =
  result = newSeq[float](filterbank.fbank.len)
  for i in 0..<filterbank.fbank.len:
    let magnitudeSquared = filterbank.fbank[i].q1 * filterbank.fbank[i].q1 + 
                         filterbank.fbank[i].q2 * filterbank.fbank[i].q2 - 
                         filterbank.fbank[i].q1 * filterbank.fbank[i].q2 * filterbank.fbank[i].coeff
    result[i] = sqrt(magnitudeSquared)




when isMainModule:
  randomize()

  # Simulate an image to extract data from it and use those in the Goertzel filter
  # to filter white noise.
  
  type
    GrayscaleImage = object
      width, height: int
      data: seq[uint16]  # 0-255 grayscale values
  
  
  proc newGrayscaleImage(width, height: int): GrayscaleImage =
    result.width = width
    result.height = height
    result.data = newSeq[uint16](width * height)

  
  proc getPixel(img: GrayscaleImage, x, y: int): uint16 =
    if x >= 0 and x < img.width and y >= 0 and y < img.height:
      return img.data[y * img.width + x]
    return 0

  
  proc setPixel(img: var GrayscaleImage, x, y: int, value: uint16) =
    if x >= 0 and x < img.width and y >= 0 and y < img.height:
      img.data[y * img.width + x] = value
 

  proc generateTestImage(width, height: int): GrayscaleImage =
    result = newGrayscaleImage(width, height)
    for x in 0..<width:
      for y in 0..<height:
        # Pattern: Sine wave across frequency
        let value = 128 + (127.0 * sin(float(y) / float(height) * Pi * 8.0 + float(x) / 10.0)).int   
        result.setPixel(x, y, uint16(value))
  
  
  proc updateGainsFromImageColumn(filterbank: var GoertzelFilterBank, img: GrayscaleImage, column: int) =
    let numFilters = filterbank.fbank.len  
    for i in 0..<numFilters:
      # Map filter index to image y-coordinate (invert to make bottom=low freq, top=high freq)
      let y = img.height - 1 - int(float(i) / float(numFilters - 1) * float(img.height - 1))  
      # Get pixel value and normalize to [0.0, 1.0]
      let pixelValue = getPixel(img, column, y)
      filterbank.fbank[i].gain = float(pixelValue) / 255.0
  
  
  proc generateWhiteNoise(length: int): seq[Sample] =
    result = newSeq[Sample](length)
    for i in 0..<length:
      result[i] = Sample((rand(255)).int)
  
  
  proc normalizeMagnitudes(magnitudes: seq[float]): seq[float] =
    if magnitudes.len == 0:
      return @[] 
    let (_, maxMag) = minmax(magnitudes)
    # Avoid division by zero
    if maxMag == 0:
      return newSeq[float](magnitudes.len)
    result = newSeq[float](magnitudes.len)
    for i in 0..<magnitudes.len:
      result[i] = magnitudes[i] / maxMag
  
  
  proc visualizeAsciiArt(filterbank: GoertzelFilterBank, magnitudes: seq[float]) =
    echo "Frequency | Gain | Magnitude"
    echo "-------------------------"
    let normMagnitudes = normalizeMagnitudes(magnitudes)
    const barWidth = 40
    for i in 0..<filterbank.fbank.len:
      if i mod (filterbank.fbank.len div 50) == 0:  # Display subset of filters for readability
        let 
          freq = filterbank.fbank[i].frequency
          gain = filterbank.fbank[i].gain
          mag = normMagnitudes[i]
          gainBars = int(gain * (barWidth / 2))
          magBars = int(mag * barWidth)        
        echo fmt"{freq:7.1f} Hz | {gain:4.2f} {'.'.repeat(gainBars)} | {magnitudes[i]:8.2f} {'#'.repeat(magBars)}"
  
  
  # Process the white noise signal through filters with gains controlled by an image column
  proc processImageColumnFilter(filterbank: var GoertzelFilterBank, img: GrayscaleImage, column: int) =
    echo fmt"Processing image column {column}:"
    filterbank.updateGainsFromImageColumn(img, column)
    let whiteNoise = generateWhiteNoise(filterbank.fftSize)
    filterbank.resetFilterbank()
    for sample in whiteNoise:
      filterbank.processAllFilters(sample)
    let magnitudes = filterbank.getAllMagnitudes()
    filterbank.visualizeAsciiArt(magnitudes)
  

  proc main() =
    # Number of filters (30 per octave)
    let octaves = 8  # 8 octaves from ~50Hz to ~15kHz
    let filtersPerOctave = 30
    let totalFilters = octaves * filtersPerOctave
    var filterbank = initFilterbank(
      minFreq = 50.0, 
      octaves = octaves, 
      nFiltersOct = filtersPerOctave, 
      fftSize = 1024 
    )
    
    # Create test "image" (200 frames/columns wide, height == filter count)
    let img = generateTestImage(200, totalFilters)
    
    echo "Image-Controlled Hopping Goertzel Filterbank"
    echo fmt"Sampling Rate: {SampleRate} Hz"
    echo fmt"Frequency Range: {MinFreq} Hz to {MaxFreq} Hz"
    echo fmt"Filters: {totalFilters} total ({filtersPerOctave} per octave)"
    echo ""
    
    # few columns for demonstration
    for col in countup(0, 190, 10):
      filterbank.processImageColumnFilter(img, col)
      echo ""

  main()

goerzelfilter.nim

import strutils
import math

const 
  SampleRate = 8000.0  # 8kHz
  TargetFrequency = 941.0  # 941 Hz
  FFTSize = 205
  N = FFTSize  # Block size

type
  Sample = uint8
  
  GoertzelFilter = object
    frequency: float
    coeff: float
    sine, cosine: float
    s0, s1, s2: float


proc initGoertzel(frequency: float): GoertzelFilter =
  let 
    floatN = float(N)
    frequencyIndex = int(0.5 + (floatN * TargetFrequency) / SampleRate)
    omega = (2.0 * Pi * frequencyIndex.float) / floatN
  
  result = GoertzelFilter(
    frequency: frequency,
    sine: sin(omega),
    cosine: cos(omega),
    s0: 0.0,
    s1: 0.0,
    s2: 0.0,
  )
  result.coeff = 2.0 * result.cosine

 
# Reset before every "block" (size=N) of samples.
proc resetGoertzel(gf: var GoertzelFilter) =
  gf.s0 = 0
  gf.s2 = 0
  gf.s1 = 0


# Every sample.
proc processSample(gf: var GoertzelFilter, sample: Sample) =
  gf.s0 = gf.coeff * gf.s1 - gf.s2 + float(sample)
  gf.s2 = gf.s1
  gf.s1 = gf.s0


# After every processed block to get the complex result.
proc getRealImag(gf: GoertzelFilter, realPart, imagPart: var float) =
  realPart = (gf.s1 - gf.s2 * gf.cosine)
  imagPart = (gf.s2 * gf.sine)


# After every processed block to get the relative magnitude squared.
proc getMagnitudeSquared(gf: GoertzelFilter): float =
  result = gf.s1 * gf.s1 + gf.s2 * gf.s2 - gf.s1 * gf.s2 * gf.coeff


#=====
proc generate(testData: var openarray[Sample], frequency: float) =
  let step = frequency * ((2.0 * Pi) / SampleRate)  
  for index in 0..<N:
    testData[index] = Sample(100.0 * sin(float(index) * step) + 100.0)


proc generateAndTest(frequency: float) = 
  echo "For test frequency ", frequency, ":"

  var gf = initGoertzel(frequency)  
  var testData: array[N, Sample]
  testData.generate(frequency)

  # Process the samples
  for index in 0..<N:
    gf.processSample(testData[index])
  
  # Do the "basic Goertzel" processing.
  var real, imag: float
  gf.getRealImag(real, imag)
  
  echo "real = ", real, " imag = ", imag
  
  let magnitudeSquared = real*real + imag*imag
  echo "Relative magnitude squared = ", magnitudeSquared
  
  let magnitude = sqrt(magnitudeSquared)
  echo "Relative magnitude = ", magnitude
  
  let magnitudeSquared2 = gf.getMagnitudeSquared()
  echo "Relative magnitude squared = ", magnitudeSquared2
  
  let magnitude2 = sqrt(magnitudeSquared2)
  echo "Relative magnitude = ", magnitude2, "\n"
  
  gf.resetGoertzel()



# Demo 2
proc generateAndTest2(frequency: float) =
  var gf = initGoertzel(frequency)

  stdout.write "Freq=" & align($frequency, 7, ' ') & "   "
  
  var testData: array[N, Sample]
  testData.generate(frequency)
  
  # Process the samples.
  for index in 0..<N:
    gf.processSample(testData[index])
  
  var real, imag: float
  gf.getRealImag(real, imag)
  
  let magnitudeSquared = real*real + imag*imag
  stdout.write "rel mag^2=" & align($magnitudeSquared, 16, ' ') & "   "
  
  let magnitude = sqrt(magnitudeSquared)
  echo "rel mag=" & align($magnitude, 12, ' ')
  
  gf.resetGoertzel()

proc main() =
  generateAndTest(TargetFrequency - 250.0)
  generateAndTest(TargetFrequency)
  generateAndTest(TargetFrequency + 250.0)  
  # Demo 2
  var freq = TargetFrequency - 300.0
  while freq <= TargetFrequency + 300.0:
    generateAndTest2(freq)
    freq += 15

main()