/* ReceiverFM.ino      Bob Larkin   26 April 2020
 * No copyright.
 * 
 * This INO runs the FM detector on input data from the Codec. It
 * requires an FM modulated signal source at 15 kHz.  The Serial out is
 * the squelch level generated by the squelch noise detector.
 * 
 * Commented out is a test of introducing a sine wave to the
 * FM Detector and taking 512 samples of the output.  It is
 * a static test with a fixed frequency for test and so
 * the output DC value and noise can be tested.  Note that the 512
 * samples include the startup transient, so the first 300, 
 * or so, points should be ignored in seeing the DC value.
 * 
 * An option is to change the squelch noise filter (commented out)
 * and this serves as an example of changing the 10 coefficients.
 * 
 * Another commented out loop code will print the observed spectrum using
 * the 1024 point FFT.  This shows the NBFM de-emphasis curve.
 * 
 * Change the value of sine1.frequency to see the DC output change.
 * See FMReceiver2.ino for testing with real AC modulation.
 * 
 * As an alternative the input can come from the ADC for "SINE_ADC 0"
 * 
 * Output is sent to left channel SGTL5000 DAC.
 */

#include "Audio.h"
#include <OpenAudio_ArduinoLibrary.h>

// SINE_ADC 1 for internally generated sine wave.
// SINE_ADC 0 to use the SGTL5000 Teensy audio adaptor ADC/DAC
#define SINE_ADC 0

#if SINE_ADC
AudioSynthGaussian_F32      gwn1;
AudioSynthWaveformSine_F32  sine1;
AudioMixer4_F32             mix1;
RadioFMDetector_F32         fmDet1;
AudioOutputI2S_F32          i2sOut;
AudioAnalyzeFFT1024_F32     fft1;
AudioRecordQueue_F32        queue1;
AudioControlSGTL5000        sgtl5000_1;
AudioConnection_F32     connectA(gwn1,   0, mix1,   0);
AudioConnection_F32     connectB(sine1,  0, mix1,   1);
AudioConnection_F32     connectC(mix1,   0, fmDet1, 0);
#else            // Input from Teensy Audio Adaptor SGTL5000
// Note - With no input, the FM detector output is all noise.  This
// can be loud, so lower the gain below (like 0.05 gain).
AudioInputI2S_F32         i2sIn;
RadioFMDetector_F32       fmDet1;
AudioAnalyzeFFT1024_F32   fft1;
AudioRecordQueue_F32      queue1; 
AudioOutputI2S_F32        i2sOut;
AudioControlSGTL5000      sgtl5000_1;
AudioConnection_F32     connectD(i2sIn,   0, fmDet1,  0); // left
#endif
// Common for both input sources
AudioConnection_F32     connect1(fmDet1,  1, i2sOut,  0); // Squelched
AudioConnection_F32     connect2(fmDet1,  0, fft1,    0);
AudioConnection_F32     connect3(fmDet1,  0, queue1,  0);

float dt1[512];    // Place to save output
float *pq1, *pd1;
uint16_t k;
int i;

void setup(void) {
  AudioMemory(5);
  AudioMemory_F32(100);
  Serial.begin(300);  delay(500);  // Any rate is OK
  Serial.println("Serial Started");
 
  sgtl5000_1.enable();
  sgtl5000_1.inputSelect(AUDIO_INPUT_LINEIN);   

#if SINE_ADC
  sine1.frequency(15000.0);
  sine1.amplitude(0.0001f);
  gwn1.amplitude(0.1f);
#endif

  fft1.setOutputType(FFT_DBFS);
  fft1.setNAverage(10);

  // The FM detector has error checking during object construction
  // when Serial.print is not available.  See RadioFMDetector_F32.h:
  Serial.print("FM Initialization errors: ");
  Serial.println( fmDet1.returnInitializeFMError() );

  // The following enables error checking inside of the "ubdate()"
  // Output goes to the Serial (USB) Monitor.  Normally, this is quiet.
  fmDet1.showError(1);

  fmDet1.setSquelchThreshold(0.7f);

  // Here is an example of designing and using a non-default squelch
  // noise filter.  The INO provides storage for the new coefficients.
//  static float newFilter[10];
  //                  freq     Q    ptr to Coeff
//  setBandpassBiQuad(2000.0f, 3.0f, &newFilter[0]);
//  setBandpassBiQuad(4000.0f, 3.0f, &newFilter[5]);
//  fmDet1.setSquelchFilter(newFilter);
  
  // Set the volume control (0.0 to 1.0)
  i2sOut.setGain(0.05f);

  queue1.begin(); 
  i = 0;  k=0;
}

void loop(void) {
 float lData[512];

  Serial.print("sqLevel "); Serial.println(fmDet1.getSquelchLevel(), 6);
  delay(500);
 
/*
 if( fft1.available() ) {
    float* pd = fft1.getData();
    for(int k=0; k<512; k++)
        lData[k] = pd[k];
    for(int k=0; k<512; k++)
        Serial.println(lData[k],3);
    Serial.println(" -------");
    }
 */

/*
  // Collect 512 samples and output to Serial
  // This "if" will be active for i = 0,1,2,3
  if (queue1.available() >= 1) {
    if( i>=0 && i<4) {
      pq1 = queue1.readBuffer();
      pd1 = &dt1[i*128];
      for(k=0; k<128; k++)
         *pd1++ = *pq1++; 
      queue1.freeBuffer();
      if(i++==3) {
         i=4;   // Only collect 4 blocks
         queue1.end();  // No more data to queue1
         }
      }
    else {
      queue1.freeBuffer();
      }
    }
  // We have 512 data samples.  Serial.print them
  if(i == 4) {
#if SINE_ADC
	Serial.println("For 14,000 Hz sine wave input:");
#endif
	Serial.println("512 samples of FM Det output, starting t=0");
    Serial.println("Time in sec, FM Output, Dev from 15,000 Hz:");
    for (k=0; k<512; k++) {
       Serial.print (0.000022667*(float32_t)k, 6);   Serial.print (", ");
       Serial.print (dt1[k],7);   Serial.print (", ");
       Serial.println (dt1[k]/0.000142421, 2);  // Convert to Hz
       }
    i = 5;
    }
  if(i==5) {
    i = 6;
    Serial.print("CPU: Percent Usage, Max: ");
    Serial.print(AudioProcessorUsage());
    Serial.print(", ");
    Serial.println(AudioProcessorUsageMax());

    Serial.print("Int16 Memory Usage, Max: ");
    Serial.print(AudioMemoryUsage());
    Serial.print(", ");
    Serial.println(AudioMemoryUsageMax());

    Serial.print("Float Memory Usage, Max: ");
    Serial.print(AudioMemoryUsage_F32());
    Serial.print(", ");
    Serial.println(AudioMemoryUsageMax_F32());
    Serial.println();
    }
 */
  }

    // Find BiQuad Coefficients for Squelch Noise filter.
    // Only used if the default -6 dB points of 2680 and 4420 Hz
    // are not suitable
    void setBandpassBiQuad(float frequency, float q, float* cf) {
        double w0 = frequency * (2 * 3.141592654 / 44100);   // sampleRate_Hz);
        double sinW0 = sin(w0);
        double alpha = sinW0 / ((double)q * 2.0);
        double cosW0 = cos(w0);
        double scale = 1.0 / (1.0+alpha);
        /* b0 */ cf[0] = alpha * scale;
        /* b1 */ cf[1] = 0;
        /* b2 */ cf[2] = (-alpha) * scale;
        /* a1 */ cf[3] = -(-2.0 * cosW0) * scale;
        /* a2 */ cf[4] = -(1.0 - alpha) * scale;
        }
