#include <Audio.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <SerialFlash.h>
#include <Metro.h>

//Sensor smoothing and hysteresys
#include <SetPoint.h>
#include <ResponsiveAnalogRead.h>


/*    ESP commands
 * AT+MODE=1
 * AT+CWLAP lists wifi AP
 * AT+CWJAP="IM_AP","raspberry"   connect to IM ap
 * AT+CIFSR     get IP
 * AT+CIPSTART="UDP","192.168.3.1",8000     connect to **
 * 
 */

 
/* SdWav1 : sample voix           |
 * SdWav2 : sample attente        |-> mix1 -> Lout & RMS1
 * 
 * SdWav3 : sample ambiance       -> mix2 -> Rout  & RMS2
 */
//// GUItool: begin automatically generated code
//AudioPlaySdWav           LoopPlayer;     //xy=152,429   LOOP
//AudioPlaySdWav           PresencePlayer;     //xy=153,217   PRESENCE
//AudioPlaySdWav           WaitPlayer;     //xy=155,307   WAIT
//AudioMixer4              mixer1;         //xy=572,215
//AudioMixer4              mixer2;         //xy=579,344
//AudioAnalyzeRMS          rms1;           //xy=875,315
//AudioOutputI2S           i2s1;           //xy=876,225
//AudioAnalyzeRMS          rms2;           //xy=876,365
//AudioConnection          patchCord1(LoopPlayer, 0, mixer2, 0);
//AudioConnection          patchCord2(PresencePlayer, 0, mixer1, 0);
//AudioConnection          patchCord3(WaitPlayer, 0, mixer1, 1);
//AudioConnection          patchCord4(mixer1, 0, i2s1, 0);
//AudioConnection          patchCord5(mixer1, rms1);
//AudioConnection          patchCord6(mixer2, 0, i2s1, 1);
//AudioConnection          patchCord7(mixer2, rms2);
//AudioControlSGTL5000     sgtl5000_1;     //xy=573,537
//// GUItool: end automatically generated code


// GUItool: begin automatically generated code
AudioPlaySdWav           LoopPlayer;     //xy=153,217
AudioPlaySdWav           PresencePlayer;     //xy=155,307
AudioMixer4              mixer1;         //xy=572,215
AudioMixer4              mixer2;         //xy=579,344
AudioAnalyzeRMS          rms1;           //xy=875,315
AudioOutputI2S           i2s1;           //xy=876,225
AudioAnalyzeRMS          rms2;           //xy=876,365
AudioConnection          patchCord1(PresencePlayer, 0, mixer1, 0);
AudioConnection          patchCord2(LoopPlayer, 0, mixer1, 1);
AudioConnection          patchCord3(LoopPlayer, 1, mixer2, 0);
AudioConnection          patchCord4(mixer1, 0, i2s1, 0);
AudioConnection          patchCord5(mixer1, rms1);
AudioConnection          patchCord6(mixer2, 0, i2s1, 1);
AudioConnection          patchCord7(mixer2, rms2);
AudioControlSGTL5000     sgtl5000_1;     //xy=573,537
// GUItool: end automatically generated code

// Config variables
int configPort = 8001 ;
String ID = "ina" ;
//float hysteresis = 5;

bool UDPalive ;

Metro checkUDP = Metro(1000);
Metro checkESPserial = Metro(50);
Metro sendRMS = Metro(50);

//#define LOOP_WAV "LOOP2.WAV"
//#define PRESENCE_WAV "PRESENCE.WAV"
//#define WAIT_WAV "WAIT.WAV"

String LOOP_WAV;
String PRESENCE_WAV ;

#define configFileName "config.txt"

int WAIT_TIME=10000 ;

const int LDR_PIN = A7;
ResponsiveAnalogRead ldr(LDR_PIN, false);
float threshold = 935;
float hysteresis = 5;
int sensorPin = A7;
SetPoint setPoint;

bool presence ;
unsigned long int LastPresence ;

 
// Use these with the Teensy Audio Shield
#define SDCARD_CS_PIN    10
#define SDCARD_MOSI_PIN  7
#define SDCARD_SCK_PIN   14

////////////////////////////// CONFIG FUNCTIONS  ///////////////////////////

String getSubstring(String dataString, char separator, int index)
{
    int found = 0;
    int strIndex[] = { 0, -1 };
    int maxIndex = dataString.length() - 1;

    for (int i = 0; i <= maxIndex && found <= index; i++) {
        if (dataString.charAt(i) == separator || i == maxIndex) {
            found++;
            strIndex[0] = strIndex[1] + 1;
            strIndex[1] = (i == maxIndex) ? i+1 : i;
        }
    }
    return found > index ? dataString.substring(strIndex[0], strIndex[1]) : "";
}

bool readSDconfig(){
  File configFile = SD.open(configFileName);
  while (configFile.available()) {                  //as long as there is data available
    String buf = configFile.readStringUntil('\n');  // read a line
    String keyword = getSubstring(buf, ' ', 0);     // update variable according to keyword
    if (keyword == "port") { configPort = getSubstring(buf, ' ', 1).toInt() ; Serial.print("port : ") ; Serial.println(configPort); }
    if (keyword == "hysteresis") { hysteresis = getSubstring(buf, ' ', 1).toInt() ; Serial.print("hysteresis : ") ; Serial.println(hysteresis);}
    if (keyword == "ID") {
      ID = getSubstring(buf, ' ', 1) ; 
      Serial.print("ID : ") ; 
      Serial.println(ID);
      LOOP_WAV = ID + "_L.wav";
      PRESENCE_WAV = ID + "_A.wav" ;
      }
  }
  configFile.close();
}
  
////////////////////////////// LDR & AUDIO FUNCTIONS  ///////////////////////////


void playFile(const char *filename)
{
  Serial.print("Playing file: ");
  Serial.println(filename);
  PresencePlayer.play(filename);
  // A brief delay for the library read WAV info
  delay(5);

}

void printLDR(){
  
  Serial.print(analogRead(A3));
  Serial.print("   ");
  Serial.print(digitalRead(16));
  Serial.print("   ");
  Serial.print(analogRead(A7));
  Serial.print("   ");
  Serial.print(digitalRead(20));
  Serial.println();
}


bool checkPresence(int Pin1, int Pin2) {
  bool isPresent = analogRead(A7)>1010;
  //digitalRead(Pin2); //|| digitalRead(Pin2);
  Serial.println(isPresent);
  return isPresent ;
}

////////////////////////////// ESP FUNCTIONS  ///////////////////////////


void ESPSerial(){
   // Send bytes from ESP8266 -> Teensy to Computer
   bool printLine ;
    while ( Serial1.available() ) {
      //String buf = Serial1.read();
      //Serial.write(F(buf));
        Serial.write( Serial1.read() );
    //    if(buf.readStringUntil("\n")=="OK") {Serial.println("trouvé");}
        printLine = 1 ;
    }
//    if (printLine) {Serial.println("");}
    printLine = 0;
    // Send bytes from Computer -> Teensy back to ESP8266
    while ( Serial.available() ) {
        Serial1.write( Serial.read() );
    }
}



bool checkUDPconnected() {
  Serial1.println("AT+CIPSTATUS");
  delay(50);
  while ( Serial1.available() ) {
    String buf = Serial1.readStringUntil('\n'); //StringUntil('\n') ;
    if(buf.startsWith("STATUS:2")){
      Serial.println("connected");
      return true ;
    }
    else 
    {
      int port = configPort;
      Serial1.print("AT+CIPSTART=\"UDP\",\"192.168.3.1\",");
      Serial1.println(port);
      Serial.println ("Reconnecting UDP");
      ESPSerial();
      
      return false;
    }
    
  }
}
void sendUDP() {
  
  
  if(rms1.available() ) { //&& UDPalive
         Serial1.println("AT+CIPSEND=3");
         delay(20);
         Serial1.write(byte(rms1.read()*255));
         Serial1.println("");
         //Serial.println("udp");
       }

}

//////////////////////    LDR event handlers    /////////////////////////////

void Presence() {

  Serial.println("Presence detected");
  presence = true ;

  if (!PresencePlayer.isPlaying()) {  //!PresencePlayer.isPlaying()) {
    // Fade out wait file
    Serial.println("Fade out Wait file");
    for (float fadeRamp ; fadeRamp < 100 ; fadeRamp++ ) {
      mixer1.gain(1, 1.0 - fadeRamp/99) ;
    }    

    // start presence file
    mixer1.gain(0, 1.0) ;
    PresencePlayer.play(PRESENCE_WAV.c_str()) ;
    delay(15);
    Serial.println("Playing presence file");   
  }
}
  

void Absence() {
  LastPresence = millis() ;
  presence = false ;

  Serial.println("No presence detected");

//  if (PresencePlayer.isPlaying()) {
//      // if presence gone while playing ?
//      Serial.println("Fading out Presence file");
//      for (float fadeRamp ; fadeRamp < 100 ; fadeRamp++ ) {
//          mixer1.gain(0, 1.0 - fadeRamp/100) ;
//          delay(10);
//        }
//
//      
//      
//    }
    
  }

/////////////////////////////// SETUP /////////////////////////


void setup() {
  
  Serial.begin(115200);
  Serial1.begin(115200);
  //LDR digital inputs
  pinMode(16, INPUT);
  pinMode(20,INPUT);
  


  // load SD config file
  SPI.setMOSI(SDCARD_MOSI_PIN);
  SPI.setSCK(SDCARD_SCK_PIN);
  if (!(SD.begin(SDCARD_CS_PIN))) {
    // stop here, but print a message repetitively
    while (1) {
      Serial.println("Unable to access the SD card");
      delay(500);
    }
  }
  readSDconfig();
  delay(500);
  // Start audio
  AudioMemory(16);
  sgtl5000_1.enable();
  sgtl5000_1.lineOutLevel(13);  // set output to 3.16 Vpp
  sgtl5000_1.volume(1); //set volume to maximum
  Serial.print("Playing LOOP file ");
  Serial.println(PRESENCE_WAV);
  LoopPlayer.play(LOOP_WAV.c_str());
//  LoopPlayer.play(PRESENCE_WAV.c_str());

  // A brief delay for the library read WAV info
  delay(10); 

//  //Start sensors for calibration
//  setPoint.begin(threshold, hysteresis);
//  setPoint.attach(RISING_EDGE, Presence);
//  setPoint.attach(FALLING_EDGE, Absence);
  
  //ESP8266
  Serial1.println("AT+GMR");
  //while (!Serial1.available()) {};
  delay(200);  
  ESPSerial();  
  Serial.println("");
  Serial.println("");
  float sum ;
  float counter ;
  while(millis()<10000){ // wait for ESP to connect to its configured wifi
    //make a mean value of LDR readings in between
    ldr.update();
     sum += ldr.getValue();
     counter++;
     delay(100);
    }
    threshold = sum / counter + hysteresis/2 ;
  Serial.print("Threshold calibrated at : ");
  Serial.println(threshold);
  
  Serial1.println("AT+CIFSR");
  ESPSerial();
  delay(200);
  Serial.println ("");
  int port = configPort;
  Serial1.print("AT+CIPSTART=\"UDP\",\"192.168.3.1\",");
  Serial1.println(port);
  ESPSerial();
  Serial.println ("");

  // Start sensors handlers
  setPoint.begin(threshold, hysteresis);
  setPoint.attach(RISING_EDGE, Presence);
  setPoint.attach(FALLING_EDGE, Absence);
  setPoint.update(0); // force no presence state at startup
}

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





///////////////////////////////  LOOP ////////////////////////////////////

void loop() {
  
  // LOOP FILE

  ldr.update();
  int value = ldr.getValue();
  //Serial.println(value);
  setPoint.update(value);


  // LOOP is always playing
  if (!LoopPlayer.isPlaying()) {
    Serial.println("Playing LOOP file");
    LoopPlayer.play(LOOP_WAV.c_str());
    // A brief delay for the library read WAV info
    delay(10); 
  }

  
  if (presence ) {      //|| millis()-LastPresence < WAIT_TIME
      // update presence timer
      LastPresence = millis() ;
      // restart presence player if ended
      if (!PresencePlayer.isPlaying()) {
        PresencePlayer.play(PRESENCE_WAV.c_str());
        delay(10);
      }  
  }
  
   //WAIT fades up if no presence during longer than WAIT_TIME
  if (millis()-LastPresence > WAIT_TIME && presence == false ) { // && 
    
    if (PresencePlayer.isPlaying()){
      Serial.println("Stopping PRESENCE file");
      
      Serial.println("Fading down presence and fading up WAIT file");
      for (float fadeRamp ; fadeRamp < 100 ; fadeRamp++ ) {
            mixer1.gain(1, fadeRamp/99) ;
            mixer1.gain(0, 1.0 - fadeRamp/99) ;
            delay(10);
      }
    }
//      Serial.println("Stopping PRESENCE file");
//      
//      Serial.println("Fading down presence and fading up WAIT file");
//      for (float fadeRamp ; fadeRamp < 100 ; fadeRamp++ ) {
//            mixer1.gain(1, fadeRamp/100) ;
//            mixer1.gain(0, 1.0 - fadeRamp/100) ;
//            
//      }      
      PresencePlayer.stop();
      delay(10);
      Serial.println(PresencePlayer.isPlaying());
//    }
//    PresencePlayer.stop();
  }

//  // PRESENCE FILE
//  presence = checkPresence(16, 20);
//  //printLDR();
//  //Serial.println(presence);
//
//  //PRESENCE DETECTED
//  if (presence ) { 
//    
//  else if (!presence ) {
    
    

  if (checkUDP.check() ==1) {
    UDPalive = checkUDPconnected();
  }
  if (checkESPserial.check() == 1) {
    
  }
  ESPSerial();

 if (sendRMS.check() == 1) {
   sendUDP();
  }
  
}