// Libraries
#include<arduinoFFT.h>
#include<AccelStepper.h>
#include <TFT_eSPI.h> // Hardware-specific library
TFT_eSPI tft = TFT_eSPI();       // Invoke custom library

//PINS and parameters
#define MIC_PIN   12
//SAMPLES-pt FFT. Must   be a base 2 number. Max 128 for Arduino Uno.
#define SAMPLES 256   
//Ts = Based on Nyquist, must be 2 times the highest expected frequency.
#define SAMPLING_FREQUENCY 2048
arduinoFFT   FFT = arduinoFFT();

//Stepper declaration
const int DIR = 26; 
const int STEP = 25;
#define motorInterfaceType 1
AccelStepper motor(motorInterfaceType, STEP, DIR);
//Buttons
#define BTN1 35
#define BTN2 0
//Define colors

#define Background   TFT_BLACK      //Standby
#define Background1  0x10e4         //Meassure
#define Background2  0x02F9         //Main
#define Background3  0x1303         //Done
#define Background4  0xF163         //Error

// define the variables needed for the audio levels
int lvl = 0;
int al = 10;
unsigned int samplingPeriod;
unsigned long microSeconds;
int Analog, n, height, iterations, Position;
double F_A [25]={ };
double F_Av, Freq_Av;
bool Status = 0;
char String;
int Wait = 5000;

//create vector of size SAMPLES to hold real values
double vReal[SAMPLES]; 
//create vector of size SAMPLES to hold imaginary values
double   vImag[SAMPLES];

void setup() {
  //Begin serial communication
  Serial.begin(115200);
  //Period in microseconds  
  samplingPeriod = round(1000000*(1.0/SAMPLING_FREQUENCY));
  //Stepper setup
  motor.setMaxSpeed(1500); 
  motor.setAcceleration(100); 
  motor.setSpeed(250);
  motor.moveTo(1);
  //Button setup
  pinMode(0, INPUT_PULLUP);       
  pinMode(35, INPUT_PULLUP);
  //LCD setup
  tft.init();
  tft.setRotation(2);
}


void loop (){
    tft.setCursor(20, 100, 2);
    tft.setTextColor(TFT_WHITE,Background);  tft.setTextSize(2);
    tft.println("Standby"); 

if (digitalRead(BTN1) == 0){
    Status = 1;
    delay(500);
    Serial.println("Turned on");  
}
if (Status == 1) {
  iterations = 0;
   tft.fillScreen(Background);
   tft.setCursor(53, 100, 4);
    tft.setTextColor(TFT_WHITE,Background);  tft.setTextSize(2);
    tft.println("4"); 
    delay(1000);
    tft.fillScreen(Background);
   tft.setCursor(53, 100, 4);
    tft.setTextColor(TFT_WHITE,Background);  tft.setTextSize(2);
    tft.println("3"); 
    delay(1000);
    tft.fillScreen(Background);
   tft.setCursor(53, 100, 4);
    tft.setTextColor(TFT_WHITE,Background);  tft.setTextSize(2);
    tft.println("2"); 
    delay(1000);
    tft.fillScreen(Background);
   tft.setCursor(53, 100, 4);
    tft.setTextColor(TFT_WHITE,Background);  tft.setTextSize(2);
    tft.println("1"); 
    delay(1000);
    tft.fillScreen(Background1);
    tft.setCursor(35, 100, 2);
    tft.setTextColor(TFT_WHITE,Background1);  tft.setTextSize(2);
    tft.println("Merim");
    delay(3000);
  while (iterations<10){
  for (int i=0; i<SAMPLES; i++){  
        microSeconds = micros();    //Returns the   number of microseconds since the Arduino board began running the current script.   
        vReal[i] = analogRead(MIC_PIN); //Reads the value from analog pin   0 (A0), quantize it and save it as a real term.
        vImag[i] = 0; //Makes   imaginary term 0 always
        /*remaining wait time between samples if   necessary*/
        while(micros() < (microSeconds + samplingPeriod))
        {}//do nothing
        }
        /*Perform FFT on samples*/
    FFT.Windowing(vReal, SAMPLES, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
    FFT.Compute(vReal,   vImag, SAMPLES, FFT_FORWARD);
    FFT.ComplexToMagnitude(vReal, vImag, SAMPLES);
    /*Find peak frequency and print peak*/
    double peak = FFT.MajorPeak(vReal,   SAMPLES, SAMPLING_FREQUENCY);
    // Save meassurements into arrays
    F_A [iterations] = peak;
    iterations = iterations + 1;
    Serial.print(" F = ");
    Serial.println(peak);
    delay(50);

  }
  //Stop Sensory loop
  Status='0';
  Status = 0;
   F_Av = FindMean(F_A, al);
    Serial.println(F_Av);


  //Stepper code
if (F_Av >=90 && F_Av <=186) {
      String = 'E';
      LCD_Work(F_Av, String);
      if (F_Av<164.8){
        F_Av = F_Av - 1; 
        F_Av = F_Av/2;       
        Serial.println("Struna E je nizko");
        Position = StepFunctionE (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>164.8){
        F_Av = F_Av/2;
        Serial.println("Struna E je vysoko");
        Position = StepFunctionE (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      
      } }

if (F_Av >=196 && F_Av <= 248) {
      String = 'A';
      LCD_Work(F_Av, String);
      if (F_Av<220){
        F_Av = F_Av - 0.2;
        F_Av = F_Av/2;
        Serial.println("Struna A je nizko");
        Position = StepFunctionA (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>220){
        F_Av = F_Av - 1;
        F_Av = F_Av/2;
        Serial.println("Struna A je vysoko");
        Position = StepFunctionA (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      } 
  }



if (F_Av >=260 && F_Av <= 330) {
      String = 'D';
      LCD_Work(F_Av, String);
      if (F_Av<293.7){
        F_Av = F_Av - 0.7;
        F_Av = F_Av/2;
        Serial.println("Struna D je nizko");
        Position = StepFunctionD (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>293.7){
        F_Av = F_Av - 0.4;
        F_Av = F_Av/2;
        Serial.println("Struna D je vysoko");
        Position = StepFunctionD (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      } 
  }
  
if (F_Av >=348 && F_Av <= 440) {
      String = 'G';
      LCD_Work(F_Av, String);
      if (F_Av<392){
        F_Av = F_Av - 1.5;
        F_Av = F_Av/2;
        Serial.println("Struna G je nizko");
        Position = StepFunctionG (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>392){
        F_Av = F_Av - 0.8;
        F_Av = F_Av/2;
        Serial.println("Struna G je vysoko");
        Position = StepFunctionG (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      } 
  }

if (F_Av >=444 && F_Av <=556) {
      String = 'B';
      LCD_Work(F_Av, String);
      if (F_Av<493.9){
        F_Av = F_Av - 1.2;
        F_Av = F_Av/2;
        Serial.println("Struna B je nizko");
        Position = StepFunctionB (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>493.9){
        F_Av = F_Av - 2;
        F_Av = F_Av/2;
        Serial.println("Struna B je vysoko");
        Position = StepFunctionB (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      } 
  }  

if (F_Av >=586 && F_Av <=740) {
      String = 'e';
      LCD_Work(F_Av, String);
      if (F_Av<659.3){
        F_Av = F_Av - 1.8;
        F_Av = F_Av/2;
        Serial.println("Struna e je nizko");
        Position = StepFunctione (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        }
      }
      if (F_Av>659.3){
        F_Av = F_Av - 2;
        F_Av = F_Av/2;
        Serial.println("Struna e je vysoko");
        Position = StepFunctione (F_Av);
        Serial.println(Position);
        motor.moveTo(Position);
        delay(Wait);
        while (motor.distanceToGo() != 0 ){
          motor.run();
        }
        if(motor.distanceToGo() == 0){
          Serial.println("Dokonceno");
          LCD_Done();
          motor.moveTo(0);
        } 
      } 
  }

else if(F_Av == 0) {
  LCD_Error();
  }

}
}

int StepFunctionE (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 18.3607*Freq_input-1454.911;
  fi_2 = 18.3607*82.4-1454.911;
  d_fi = fi_1-fi_2;
  result =   d_fi/0.1125;
  return result;
}

int StepFunctionA (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 16.3241*Freq_input-1741.2364;
  fi_2 = 16.3241*110-1741.2364;
  d_fi = fi_1-fi_2;
  result =   d_fi/0.1125;
  return result;
}

int StepFunctionD (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 16.1666*Freq_input-2261.0133;
  fi_2 = 16.1666*146.83-2261.0133;
  d_fi = fi_1-fi_2;
  result =   d_fi/0.1125;
  return result;
}

int StepFunctionG (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 8.1856*Freq_input-1490.9635;
  fi_2 = 8.1856*196-1490.9635;
  d_fi = fi_1-fi_2;
  result =   (d_fi/0.1125*(-1));
  return result;
}

int StepFunctionB (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 10.5432*Freq_input-2501.541;
  fi_2 = 10.5432*246.94-2501.541;
  d_fi = fi_1-fi_2;
  result =   (d_fi/0.1125);
  return result;
}

int StepFunctione (float Freq_input){
  int result;
  float fi_1, fi_2, d_fi;
  fi_1 = 11.7348*Freq_input-3780.8065;
  fi_2 = 16.3241*110-1741.2364;
  d_fi = fi_1-fi_2;
  result = (d_fi/0.1125);
  return result;
}

void LCD_Work (float Freq_input, char Str){
  Freq_input = Freq_input/2;
  tft.fillScreen(Background2);
  tft.setCursor(30, 15, 1);
  tft.setTextColor(TFT_WHITE,Background2);  tft.setTextSize(2);
  tft.println("Struna");
  tft.setCursor(55, 50, 1);
  tft.setTextColor(TFT_WHITE,Background2);  tft.setTextSize(6);
  tft.println(Str);
  tft.setCursor(35, 100, 1);
  tft.setTextColor(TFT_WHITE,Background2);  tft.setTextSize(2);
  tft.println(F_Av);
  tft.setCursor(55, 125, 1);
  tft.println("Hz");
  tft.setCursor(10, 170, 2);
  tft.setTextColor(TFT_RED,Background2);  tft.setTextSize(2);
  tft.println("Pracuji...");
}

void LCD_Done(){tft.fillScreen(Background3);
  tft.setCursor(25, 100, 2);
  tft.setTextColor(TFT_WHITE,Background3);  tft.setTextSize(2);
  tft.println("Hotovo");
  delay(5000);
  tft.fillScreen(Background);
}

void LCD_Error(){
  tft.fillScreen(Background4);
  tft.setCursor(25, 100, 2);
  tft.setTextColor(TFT_BLACK,Background4);  tft.setTextSize(2);
  tft.println("Chyba");
  delay(5000);
  tft.fillScreen(Background);
}

double FindMean (double F_A[10], int al){
  double Average = 0;
  double Av_base = 0;
  int denominator = 0; 
  int ErrIter = 0;

  double F_AR [10] = {};
  int F_AR1 [10] = {};
  int quantity [10] = {} ;

  for (int k = 0; k<al; k++){
    F_AR1[k] = F_A[k]/10;
    F_AR[k] = F_AR1[k]*10;  
  } 

  for (int kk  =0; kk<al; kk++){
    int focused = F_AR[kk];
    int count = 1;
      for (int kl = 0; kl<al; kl++){
        if (kl!=kk){
          int subtract = focused - F_AR[kl];
          if (subtract == 0){
            count = count + 1;
          }
          quantity[kk] = count; 
        }
      }
  }

  for(int kj = 0; kj < al; kj++){
    int check = quantity[kj];
    if(check > 2){
      Av_base = Av_base + F_A[kj];
      denominator = denominator + 1;
    }
    Average = Av_base/denominator;
    if (check <= 2){
      ErrIter = ErrIter + 1;
    }
  }

  if (ErrIter >= 2){
    return 0;
  }
  else{
  return Average;
  }
}





