//Based off the code that was used in https://www.norwegiancreations.com
#include "arduinoFFT.h"

#define SAMPLES 128             //Must be a power of 2
#define SAMPLING_FREQUENCY 9600 //Hz, must be less than 10000 due to ADC



long int currentTime;

 int lowbandmaxvalue = 0; 
 int midbandmaxvalue = 0;
 int highbandmaxvalue = 0;

arduinoFFT FFT = arduinoFFT();

unsigned int sampling_period_us;
unsigned long microseconds;

double vReal[SAMPLES];
double vImag[SAMPLES];

//////////////////////////////////////////////////////////////////////////////////////////

void setup() {
  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);

  Serial.begin(115200);

  sampling_period_us = round(1000000*(1.0/SAMPLING_FREQUENCY));
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

void loop() {
  currentTime = millis();
  
  //sampling
  for(int i=3; i<SAMPLES; i++){
    microseconds = micros(); // Overflows after around 70 minutes!
    while(micros() < (microseconds + sampling_period_us)){
      
    }
    vReal[i] = analogRead(A0);
    Serial.println(vReal[i]); // Send data via serial for Matlab
    vImag[i] = 0;
    
  }

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  /*Runing the FFT*/
  FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(vReal, vImag, SAMPLES, FFT_FORWARD);
  FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);
  double peak = FFT.MajorPeak(vReal, SAMPLES, SAMPLING_FREQUENCY);

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  lowbandmaxvalue = 0;
  midbandmaxvalue = 0;
  highbandmaxvalue = 0;

  //For low band
  for(int i = 3; i < 25;i++){
    if(vReal[i] > lowbandmaxvalue){
      lowbandmaxvalue = vReal[i];  
    }
  }

  //For Mid band
  for(int j = 26; j < 45;j++){
    if(vReal[j] > midbandmaxvalue){
      midbandmaxvalue = vReal[j];  
    } 
  }
  //For High band  
  for(int k = 46; k < 63;k++){
    if(vReal[k] > highbandmaxvalue){
      highbandmaxvalue = vReal[k];  
    }  
  }
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //For Low band
  if(lowbandmaxvalue > 400 && lowbandmaxvalue < 600){
   digitalWrite(2, HIGH);
   digitalWrite(3, LOW);
   digitalWrite(4, LOW);
   digitalWrite(5, LOW);
  }

  else if(lowbandmaxvalue > 600 && lowbandmaxvalue < 1200){
   digitalWrite(2, HIGH);
   digitalWrite(3, HIGH);
   digitalWrite(4, LOW);
   digitalWrite(5, LOW);
  }

  else if(lowbandmaxvalue > 1200 && lowbandmaxvalue < 1800){
   digitalWrite(2, HIGH);
   digitalWrite(3, HIGH);
   digitalWrite(4, HIGH);
   digitalWrite(5, LOW);
  }
  
   else if(lowbandmaxvalue > 1800){
   digitalWrite(2, HIGH);
   digitalWrite(3, HIGH);
   digitalWrite(4, HIGH);
   digitalWrite(5, HIGH);
  }
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //For Mid band
  else if(midbandmaxvalue > 200 && midbandmaxvalue < 400){
   digitalWrite(6, HIGH);
   digitalWrite(7, LOW);
   digitalWrite(8, LOW);
   digitalWrite(9, LOW);
  }

  else if(midbandmaxvalue > 400 && midbandmaxvalue < 1000){
   digitalWrite(6, HIGH);
   digitalWrite(7, HIGH);
   digitalWrite(8, LOW);
   digitalWrite(9, LOW);
  }

  else if(midbandmaxvalue > 1000 && midbandmaxvalue < 1600){
   digitalWrite(6, HIGH);
   digitalWrite(7, HIGH);
   digitalWrite(8, HIGH);
   digitalWrite(9, LOW);
  }
  
   else if(midbandmaxvalue > 1600){
   digitalWrite(6, HIGH);
   digitalWrite(7, HIGH);
   digitalWrite(8, HIGH);
   digitalWrite(9, HIGH);
  }
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  //For Mid band
  if(highbandmaxvalue > 200 && highbandmaxvalue < 400){
   digitalWrite(10, HIGH);
   digitalWrite(11, LOW);
   digitalWrite(12, LOW);
   digitalWrite(13, LOW);
  }

  else if(highbandmaxvalue > 400 && highbandmaxvalue < 1000){
   digitalWrite(10, HIGH);
   digitalWrite(11, HIGH);
   digitalWrite(12, LOW);
   digitalWrite(13, LOW);
  }

  else if(highbandmaxvalue > 1000 && highbandmaxvalue < 1600){
   digitalWrite(10, HIGH);
   digitalWrite(11, HIGH);
   digitalWrite(12, HIGH);
   digitalWrite(13, LOW);
  }
  
   else if(highbandmaxvalue > 1600){
   digitalWrite(10, HIGH);
   digitalWrite(11, HIGH);
   digitalWrite(12, HIGH);
   digitalWrite(13, HIGH);
  }

  else{
   digitalWrite(2, LOW);
   digitalWrite(3, LOW);
   digitalWrite(4, LOW);
   digitalWrite(5, LOW);   
   digitalWrite(6, LOW);
   digitalWrite(7, LOW);
   digitalWrite(8, LOW);
   digitalWrite(9, LOW);   
   digitalWrite(10, LOW);
   digitalWrite(11, LOW);
   digitalWrite(13, LOW);
   digitalWrite(14, LOW);  
  }  


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  
// for(int i=0; i<(SAMPLES/2); i++){
// /*View all these three lines in serial terminal to see which frequencies has which amplitudes*/
// 
// Serial.print((i * 1.0 * SAMPLING_FREQUENCY) / SAMPLES, 1);
// Serial.print(" ");
// Serial.println(vReal[i], 1); //View only this line in serial plotter to visualize the bins
// 
// }
/*PRINT RESULTS*/
//Serial.println("time: ");
//Serial.println(millis() - currentTime); 
delay(100);
}