// FFT to select freq. bins from signal on Teensy 3.2 pin A2 (ADC input)
// Set OUT_ALARM pin high if signal in some freq. bins over threshold
// Thanks to Teensy forum users: neutronned and Frank B.

#include <Audio.h>

AudioInputAnalog         adc1;       
AudioAnalyzeFFT1024      myFFT;
AudioConnection  patchCord1(adc1, myFFT);

#define A_GAIN (5.0)    // gain applied to analog input signal
#define THRESH (0.5)    // threshold above which "movement" detected  
#define LED1 (13)       // onboard signal LED
#define OUT_ALARM (3)   // alarm signal (motion detected)
#define CPRINT (12)      // frequency bins (columns) to print

float level[16];  // frequency bins
float alarm;
static float fa = 0.10;  // smoothing fraction for low-pass filter
int loops = 0;   // how many times through loop
boolean motion = false;  // if motion detected

void setDACFreq(int freq) {  // change sampling rate of internal ADC and DAC
const unsigned config = PDB_SC_TRGSEL(15) | PDB_SC_PDBEN | PDB_SC_CONT | PDB_SC_PDBIE | PDB_SC_DMAEN;
    PDB0_SC = 0; //<--add this line
    PDB0_IDLY = 1;
    PDB0_MOD = round((float)F_BUS / freq ) - 1;    
    PDB0_SC = config | PDB_SC_LDOK;
    PDB0_SC = config | PDB_SC_SWTRIG;
    PDB0_CH0C1 = 0x0101;    
}

void setup() {
  AudioMemory(12);
  AudioNoInterrupts();
  setDACFreq(11025);
  myFFT.windowFunction(AudioWindowHanning1024);
  AudioInterrupts();
  
  Serial.begin(115200);
  digitalWrite(LED1, false);
  pinMode(LED1, OUTPUT);
  digitalWrite(OUT_ALARM, false);
  pinMode(OUT_ALARM, OUTPUT);
  digitalWrite(LED1, true);
  digitalWrite(OUT_ALARM, true);
  delay(1000);

  Serial.print("min");
  Serial.print(",");
  Serial.print("alm");
  for (int i=0; i<CPRINT; i++) {  // print column headers
      Serial.print(",");
      Serial.print("f");
      Serial.print(i);
  }
  Serial.println();
  Serial.println("# Doppler Microwave FFT on Teensy 3.1  5-AUG-2017");
  digitalWrite(LED1, false);  digitalWrite(OUT_ALARM, false);
  
} // end setup()

void loop() {
  if (myFFT.available()) {
    level[0] = ((1.0-fa)*level[0]) + fa * myFFT.read(0) * 0.5 * A_GAIN;
    level[1] =  ((1.0-fa)*level[1]) + fa * myFFT.read(1) * 1.5 * A_GAIN;
    level[2] =  ((1.0-fa)*level[2]) + fa * myFFT.read(2) * 2 * A_GAIN;
    level[3] =  ((1.0-fa)*level[3]) + fa * myFFT.read(3) * 3.5 * A_GAIN;
    level[4] =  ((1.0-fa)*level[4]) + fa * myFFT.read(4, 5) * 5 * A_GAIN;
    level[5] =  ((1.0-fa)*level[5]) + fa * myFFT.read(6, 8) * 7 * A_GAIN;
    level[6] =  ((1.0-fa)*level[6]) + fa * myFFT.read(9, 13) * 9 * A_GAIN;
    level[7] =  ((1.0-fa)*level[7]) + fa * myFFT.read(14, 22) * 11.0 * A_GAIN;
    level[8] =  ((1.0-fa)*level[8]) + fa * myFFT.read(23, 40) * 15 * A_GAIN;
    level[9] =  ((1.0-fa)*level[9]) + fa * myFFT.read(41, 66) * 15 * A_GAIN;
    level[10] = ((1.0-fa)*level[10]) + fa * myFFT.read(67, 93) * 18 * A_GAIN;
    level[11] = ((1.0-fa)*level[11]) + fa * myFFT.read(94, 131) * 10 * A_GAIN;
    alarm =  level[1] + level[2] + (2*level[3]) + (2*level[4]);

    if (alarm > THRESH) {
      motion = true;
      digitalWrite(OUT_ALARM, true);
    } else {
      motion = false;
      digitalWrite(OUT_ALARM, false);
    }
    
    if ( ((loops++)%10 == 0) ) {
     Serial.print((float)millis()/60000.0);
     Serial.print(",");
     Serial.print(alarm);    
     for (int i=0; i<CPRINT; i++) {
      Serial.print(",");
      Serial.print(level[i]);
     }
     Serial.println();
    }
  }
} // end main loop()