// *"Sampling" and "FFT" sections code was developed from AudioFrequencyDetector.ino by Clyde A. Lettsome*
// For more information visit:
// clydelettsome.com/blog/2019/12/18/my-weekend-project-audio-frequency-detector-using-an-arduino/

/*  5/2  Code will listen to music using audio jack and determine notes based on the highest frequency and its bin number
    5/10 Currently works with youtube sinwave tuning videos
    5/13 Tested with microphone and LEDs
    5/14 Need to add code for min. threshold to consider a "note" (that way if there is just ambient noise where the highest
    frequency happens to land in a note bin it won't count). May want to reduce samples
    5/15 Successfully tested with motor controller.  Flute notes can be used to trigger motors
    5/16 All motors connected to Arduino and working
    5/18 Robot tested and working
    5/19 Fixed alternating on/off problem and upgraded to dynamic gain microphone
    5/22 Added low-speed forward movement to ball launch code to help keep ball positioned against the launcher ram
*/

//Definitions and libraries
#include "arduinoFFT.h"
#define FS 2000 //Hz
#define samples 128
unsigned int sampling_period;
unsigned long microseconds;
double V[samples];
double Imaginary[samples];

//Name the motor pins:
#define Motor1Enable 2
#define Motor1A 3
#define Motor1B 4
#define Motor2Enable 5
#define Motor2A 6
#define Motor2B 7
#define Motor3Enable 8
#define Motor3A 9
#define Motor3B 10
#define Motor4Enable 11
#define Motor4A 12
#define Motor4B 13
#define Launcher 50


//Initialize FFT
arduinoFFT FFT = arduinoFFT();

//Setup code:
void setup()
{
  Serial.begin(115200);
  sampling_period = round(1.0 / FS * 1000000);

  //Set pins:
  pinMode(2, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(9, OUTPUT);
  pinMode(10, OUTPUT);
  pinMode(11, OUTPUT);
  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);
  pinMode(50, OUTPUT);
}

//Code to loop:
void loop()
{
  //**********************
  //***SAMPLING SECTION***
  //**********************
  // - this section records audio samples from microphone voltage
  
  //Gather 128 samples:
  for (int i = 0; i < samples; i++)
  {
    //Take audio sample recording. Save sample time and A0:
    microseconds = micros();
    V[i] = analogRead(1);
    
    //Set to zero since only using one input.  This is used for stereo inputs but needed for the FFT:
    Imaginary[i] = 0;

    //Wait to take the next sample according to the sampling frequency
    while (micros() < (microseconds + sampling_period)) {
    }
  }
  
  //*****************
  //***FFT SECTION***
  //*****************
  // - this section converts audio samples to frequency bins and magnitudes
  
  //Compute FFT
  FFT.Windowing(V, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(V, Imaginary, samples, FFT_FORWARD);
  FFT.ComplexToMagnitude(V, Imaginary, samples);

  //***********************
  //***FILTERING SECTION***
  //***********************
  // - this section conditions the output of the FFT 
  
  //Find highest magnitude frequency bin and the average magnitude of the frequency bins
  float max_a = 0;
  int max_i = 0;
  float sum = 0;
  float average = 0;

  /*The FFT will output 64 frequency bins, labeled 0-63.  From
     experimentation I have found that the scale from A440 to A880
     lies  completely within bins 25 to 60.  To save computation time,
     this code will only look in bins 25-60.
  */

  //This loop will find the max amplitude bin and its index as well as the total sum
  for (int i = 25; i < 60; i++)
  {
    if (V[i] > max_a)
    {
      max_a = V[i];
      max_i = i;
    }
    //Track the sum to find the average later:
    sum = sum + V[i];
    
    //Serial.println(V[i], 1);  //Debugging
  }

  //Noise gate: do not count any signal less than 1200 in magnitude
  if (max_a < 1200)
    max_i = 0;

  //Signal to Noise ratio comparison: do not count any signal less than twice the average bin magnitude
  average = sum/(60-25);
  float SNR = max_a/average;
  if (SNR < 2)
    max_i = 0;

  //***************************
  //***MOTOR CONTROL SECTION***
  //***************************
  //This section maps the output of the conditioned signal to different movements
  
  //PWM supply voltages (7 for full speed, 3 for slow):
  int pwmOutput = map(7, 0, 9, 0, 255); //7V PWM supply
  int ForwardSlow = map(3, 0, 9, 0, 255); //3V PWM supply

  //***Signal mapping: ***
  //This section uses the index of the dominant bin to determine a movement
  
  //A 440: Backward
  if ((max_i >= 27) && (max_i <= 29))
  {
    Serial.println("Backward");
    //Motor 1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, HIGH);
    digitalWrite(Motor1B, LOW);
    //Motor 2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, HIGH);
    digitalWrite(Motor2B, LOW);
    //Motor 3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, HIGH);
    digitalWrite(Motor3B, LOW);
    //Motor 4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, HIGH);
    digitalWrite(Motor4B, LOW);
  }

  //C 523: Forward
  else if ((max_i >= 33) && (max_i <= 35))
  {
    Serial.println("Forward");
    //Motor1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, LOW);
    digitalWrite(Motor1B, HIGH);
    //Motor2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, LOW);
    digitalWrite(Motor2B, HIGH);
    //Motor3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, LOW);
    digitalWrite(Motor3B, HIGH);
    //Motor4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, LOW);
    digitalWrite(Motor4B, HIGH);
  }

  //D 587: Strafe Left
  else if ((max_i >= 37) && (max_i <= 39))
  {
    Serial.println("Strafe Left");
    //Motor 1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, HIGH);
    digitalWrite(Motor1B, LOW);
    //Motor2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, LOW);
    digitalWrite(Motor2B, HIGH);
    //Motor3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, LOW);
    digitalWrite(Motor3B, HIGH);
    //Motor4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, HIGH);
    digitalWrite(Motor4B, LOW);
  }

  //E 659: Strafe Right
  else if ((max_i >= 42) && (max_i <= 44))
  {
    Serial.println("Strafe Right");
    //Motor1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, LOW);
    digitalWrite(Motor1B, HIGH);
    //Motor 2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, HIGH);
    digitalWrite(Motor2B, LOW);
    //Motor 3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, HIGH);
    digitalWrite(Motor3B, LOW);
    //Motor4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, LOW);
    digitalWrite(Motor4B, HIGH);
  }

  //G 784: Rotate CW
  else if ((max_i >= 50) && (max_i <= 52))
  {
    Serial.println("Turn Clockwise");
    //Motor1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, LOW);
    digitalWrite(Motor1B, HIGH);
    //Motor2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, LOW);
    digitalWrite(Motor2B, HIGH);
    //Motor 3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, HIGH);
    digitalWrite(Motor3B, LOW);
    //Motor 4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, HIGH);
    digitalWrite(Motor4B, LOW);
  }

  //A 880: Rotate CCW
  else if ((max_i >= 57) && (max_i <= 59))
  {
    Serial.println("Turn CCW");
    //Motor 1:
    analogWrite(Motor1Enable, pwmOutput);
    digitalWrite(Motor1A, HIGH);
    digitalWrite(Motor1B, LOW);
    //Motor 2:
    analogWrite(Motor2Enable, pwmOutput);
    digitalWrite(Motor2A, HIGH);
    digitalWrite(Motor2B, LOW);
    //Motor3:
    analogWrite(Motor3Enable, pwmOutput);
    digitalWrite(Motor3A, LOW);
    digitalWrite(Motor3B, HIGH);
    //Motor4:
    analogWrite(Motor4Enable, pwmOutput);
    digitalWrite(Motor4A, LOW);
    digitalWrite(Motor4B, HIGH);
  }

  //Otherwise: stay still (no valid signal found)
  else
  {
    Serial.println("Still");
    digitalWrite(Motor1Enable, LOW);
    digitalWrite(Motor2Enable, LOW);
    digitalWrite(Motor3Enable, LOW);
    digitalWrite(Motor4Enable, LOW);
  }
  
  //***G MAJOR LAUNCHER MOTOR CONTROL***
  //Checks four bins for four notes
  //This will cause the robot to slowly move forwards to center the ball against the firing mechanism and then triggers the firing mechanism
  if ((V[25] > 350) && (V[32] > 30) && (V[38] > 405) && (V[57] > 350)) {
    Serial.println("Fire!!!");
    //Drive forwards slowly:
    //Motor1:
    analogWrite(Motor1Enable, ForwardSlow);
    digitalWrite(Motor1A, LOW);
    digitalWrite(Motor1B, HIGH);
    //Motor2:
    analogWrite(Motor2Enable, ForwardSlow);
    digitalWrite(Motor2A, LOW);
    digitalWrite(Motor2B, HIGH);
    //Motor3:
    analogWrite(Motor3Enable, ForwardSlow);
    digitalWrite(Motor3A, LOW);
    digitalWrite(Motor3B, HIGH);
    //Motor4:
    analogWrite(Motor4Enable, ForwardSlow);
    digitalWrite(Motor4A, LOW);
    digitalWrite(Motor4B, HIGH);
    delay(1000);

    //Trigger launcher motor:
    digitalWrite(Launcher, HIGH);
    delay(1000);
    
    //Stop!
    digitalWrite(Launcher, LOW);
    digitalWrite(Motor1Enable, LOW);
    digitalWrite(Motor2Enable, LOW);
    digitalWrite(Motor3Enable, LOW);
    digitalWrite(Motor4Enable, LOW);
  }
}