/*
   ESP32_Mic_SD.ino
   Compiled for Dfrobot Firebeetle 2 ESP32-E using ESP32 board support V3.0.3

  32bit STEREO samples at 16000 samples/sec
  STEREO down sampled to 4000 samples/sec and split to 2 x Mono streams
  Inside is Left, I2S_MODE, LOW
  outside is Right, I2S_MODE, HIGH
  Inside and Outside mono signals written to SD card, flushed every 10sec
  so you can power down or remove the SD card whenever you want to stop recording

  >>> Important:
  Format the SD card before use. Using SD Card Formatter from https://www.sdcard.org/downloads/formatter/
  Use a V10 rated SD card with maximum size of 32Gig.
  Writes ~113Kb per hour of recording.
*/
/*
   (c)2024 Forward Computing and Control Pty. Ltd.
   NSW Australia, www.forward.com.au
   This code is not warranted to be fit for any purpose. You may only use it at your own risk.
   This generated code may be freely used for both private and commercial use
   provided this copyright is maintained.
*/


#include "ESP_I2S.h"
#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include "SafeString.h"
#include "BufferedOutput.h"
#include "LogFile.h"
#include "writeWAV.h"
#include "Goertzel.h"
#include "loopTimer.h"

// uncomment this for extra debug output
//#define DEBUG

// for i2s mics             // Dfrobot Firebeetle 2 ESP32-E pins
const uint8_t I2S_SCK = 25; // D2   (SCK)
const uint8_t I2S_WS = 16;  // D11  (WS)
const uint8_t I2S_DIN = 26; // D3  (DO)
// const uint8_t IS2_MODE = 4; //  (SEL) Not connected, hard wired as below
// Inside Mic (LEFT) has SEL connected to GND
// Outside Mic (RIGHT) has SEL connected to 3V3

// for SD card  NOTE: do not use D9 (GPI02) as that interfers with reprogramming
const int SD_CS_pin = 13; // D7; //SD cs pin

createBufferedOutput(bufferedOut, 1024, DROP_IF_FULL, false);

static Print* debugPtr = NULL;

#define SAMPLE_FREQ (16000)
const int16_t DOWNSAMPLED_FREQ = SAMPLE_FREQ / 4;
#define DATA_BIT_WIDTH (I2S_DATA_BIT_WIDTH_32BIT)
const size_t bytesPerSample = (DATA_BIT_WIDTH / 8);
const size_t buffer_size_samples = 8192;  // 8192 samples i.e. 4096 stereo samples
const size_t buffer_size_bytes = buffer_size_samples * bytesPerSample; // number of bytes for 8192 samples i.e. 4096 stereo samples
char rec_buffer_1[buffer_size_bytes];  // note char buffer of size buffer_size
char rec_buffer_2[buffer_size_bytes];  // note char buffer of size buffer_size
char insideBuffer[buffer_size_bytes / 2]; // note mono char buffer of size buffer_size/2
char outsideBuffer[buffer_size_bytes / 2]; // note mono char buffer of size buffer_size/2

float deltaT = ((float)buffer_size_samples) / 2 / SAMPLE_FREQ; // time per rec_buffer in sec
uint32_t buffer_counter = 0; // number of buffers saved
float zeroPaddingTime = ((float)(512 - 44)) / bytesPerSample / DOWNSAMPLED_FREQ; //  time offset for leading zero padding, mono samples at 4000Hz

const char filenameInside[] = "/RecInside.wav";//The name of the saved file
const char filenameOutside[] = "/RecOutside.wav";//The name of the saved file
File fileInside;
File fileOutside;

millisDelay flushTimer;
unsigned long FLUSH_TIME_ms = 10000;

const char logFilename[] = "/log.txt";//The name of the logFile
cSF(sfLogLine, 200); // for logging

// Create an instance of the I2SClass
I2SClass i2s;

void TaskI2SRead(void *pvParameters);
TaskHandle_t i2s_read_task_handle;  // You can (don't have to) use this to be able to manipulate a task from somewhere else.


void setup() {
  // Initialize the serial port
  Serial.begin(115200);
#ifdef DEBUG
  for (int i = 10; i > 0; i--) {
    delay(500); // wait 5sec to allow use to open plotter/monitor
  }
  bufferedOut.connect(Serial);
  debugPtr = &bufferedOut; // &Serial;
  Serial.println("DEBUG enabled");
#endif

  if (!SD.begin(SD_CS_pin)) Serial.println("SD begin failed");
  while (!SD.begin(SD_CS_pin)) {
    Serial.print(".");  // in setup just print to serial unbuffered
    delay(500);
  }

  LogFile::createLogFile(logFilename); // handles null

  if (!writeWAV::writeWaveHeader(filenameInside, DATA_BIT_WIDTH, SAMPLE_FREQ / 4, 1, true)) { // padd to 512
    while (1) {
      Serial.println("writeWaveHeader Inside Error");
      delay(3000);
    }
  }
  if (!writeWAV::writeWaveHeader(filenameOutside, DATA_BIT_WIDTH, SAMPLE_FREQ / 4, 1, true)) { // padd to 512
    while (1) {
      Serial.println("writeWaveHeader Outside Error");
      delay(3000);
    }
  }

  fileInside = SD.open(filenameInside, FILE_APPEND);
  if (!fileInside) {
    while (1) {
      Serial.println("Failed to open Inside file for appending");
      delay(5000);
    }
  }
  fileOutside = SD.open(filenameOutside, FILE_APPEND);
  if (!fileOutside) {
    while (1) {
      Serial.println("Failed to open Outside file for appending");
      delay(5000);
    }
  }


  //hardwire mode one to +vcc (right) one to GND (left)  Right is outside, left is in the box.
  // Set up the pins used for audio input
  i2s.setPins(I2S_SCK, I2S_WS, -1, I2S_DIN);
  // Initialize the I2S bus in standard mode
  if (!i2s.begin(I2S_MODE_STD, SAMPLE_FREQ, DATA_BIT_WIDTH, I2S_SLOT_MODE_STEREO)) {
    while (1) {
      Serial.println("Failed to initialize I2S bus!");
      delay(5000);
    }
  }
  for (size_t i = 0; i < 10; i++) {
    i2s.readBytes(rec_buffer_1, buffer_size_bytes); // discard first 10 buffers
  }

  // This variant of task creation can also specify on which core it will be run (only relevant for multi-core ESPs)
  xTaskCreatePinnedToCore(
    TaskI2SRead, "I2S Read", 1024  // Stack size
    , NULL  // When no parameter is used, simply pass NULL
    , 1  // Priority
    , &i2s_read_task_handle  // With task handle we will be able to manipulate with this task.
    , ARDUINO_RUNNING_CORE  // Core on which the task will run
  );
  Serial.println();
  Serial.print("Recording stereo microphones to two mono WAV files in blocks of "); Serial.print(deltaT); Serial.println(" secs.");
  Serial.println("Enter s to stop run");
  flushTimer.start(FLUSH_TIME_ms);
}

volatile bool read_1_Complete = false;
volatile bool read_2_Complete = false;
volatile bool read_1_Processed = true;
volatile bool read_2_Processed = true;
volatile size_t read_size_1 = 0;
volatile size_t read_size_2 = 0;
volatile bool read_1_delayed = false;
volatile bool read_2_delayed = false;
volatile bool taskReleased = false;

void releaseTask() {
  if (!taskReleased) {
    loopTimer.clear();
    taskReleased = true;
  }
}

void TaskI2SRead(void *pvParameters) {  // This is a task.
  (void)pvParameters;
  /*
    Read the mic
  */

  while (!taskReleased) { // wait for loop to run
    delay(1);
  }

  while (1) {
    if (!read_1_Processed) {
      read_1_delayed = true;
    }
    while (!read_1_Processed) {
      delay(1);
    }
    read_1_Processed = false;
    read_size_1 = i2s.readBytes(rec_buffer_1, buffer_size_bytes);
    read_1_Complete = true;

    if (!read_2_Processed) {
      read_2_delayed = true;
    }
    while (!read_2_Processed) {
      delay(1);
    }
    read_2_Processed = false;
    read_size_2 = i2s.readBytes(rec_buffer_2, buffer_size_bytes);
    read_2_Complete = true;
  }
}

// note insideBuffer and outsideBuffer must be at least len/2 long
// returns left righ buffer size in bytes
size_t splitStereoToMono(const char* recBuffer, size_t len, const char* insideBuffer, const char* outsideBuffer) {
  int32_t *recBuffer_32 = (int32_t*)recBuffer;
  int32_t *insideBuffer_32 = (int32_t*)insideBuffer;
  int32_t *outsideBuffer_32 = (int32_t*)outsideBuffer;
  size_t len_32 = len / bytesPerSample;
  size_t idx = 0;
  for (size_t i = 0; i < len_32 / 2; i++) { // pairs of 32bit words
    insideBuffer_32[i] = recBuffer_32[idx++];
    outsideBuffer_32[i] = recBuffer_32[idx++];
  }
  return len / 2;
}


bool stopRun = false;
void loop() {
  bufferedOut.nextByteOut(); // push bytes if it will not block
#ifdef DEBUG
  loopTimer.check(Serial);  // max time here should be < 0.5sec to process and save 2 buffers
#endif
  if (flushTimer.justFinished()) {
    flushTimer.repeat();
    fileInside.flush();
    fileOutside.flush();
  }
  if (Serial.available()) {
    int c = Serial.read();
    if (((char)c) == 's' || ((char)c) == 'S') {
      stopRun = true;
    }
  }
  if (stopRun) {
    fileInside.close();
    fileOutside.close();
    while (1) {
      Serial.println(" Run Stopped.");  // stopping here so just print unbuffered
      delay(5000);
    }
  }

  releaseTask(); // only first time

  if (read_1_Complete) {
    read_1_Complete = false;
    if (read_1_delayed) {
      sfLogLine.clear();
      sfLogLine.print(buffer_counter * deltaT + zeroPaddingTime); sfLogLine.println(" sec, 1 Delayed");
      LogFile::write(sfLogLine);
      if (debugPtr) {
        debugPtr->println(sfLogLine);
      }
      read_1_delayed = false;
    }
    size_t mono_len_bytes = splitStereoToMono(rec_buffer_1, buffer_size_bytes, insideBuffer, outsideBuffer);
    size_t downSample_len_samples = Goertzel::downSample32bitBy4((int32_t*)insideBuffer, mono_len_bytes / bytesPerSample);
    // mono_len in bytes
    if (fileInside.write((uint8_t*)insideBuffer, downSample_len_samples * bytesPerSample) != (downSample_len_samples * bytesPerSample)) {
      while (1) {
        Serial.println("Inside writeBlocks Error");  // stopping here so just print unbuffered
        delay(5000);
      }
    }
    downSample_len_samples = Goertzel::downSample32bitBy4((int32_t*)outsideBuffer, mono_len_bytes / bytesPerSample);
    if (fileOutside.write((uint8_t*)outsideBuffer, downSample_len_samples * bytesPerSample) != (downSample_len_samples * bytesPerSample)) {
      while (1) {
        Serial.println("Outside writeBlocks Error");  // stopping here so just print unbuffered
        delay(5000);
      }
    }
    buffer_counter++;
    read_size_1 = 0;
    read_1_Processed = true;
  }
  if (read_2_Complete) {
    read_2_Complete = false;
    if (read_2_delayed) {
      sfLogLine.clear();
      sfLogLine.print(buffer_counter * deltaT + zeroPaddingTime); sfLogLine.println(" sec, 2 Delayed");
      LogFile::write(sfLogLine);
      if (debugPtr) {
        debugPtr->println(sfLogLine);
      }
      read_2_delayed = false;
    }
    size_t mono_len_bytes = splitStereoToMono(rec_buffer_2, buffer_size_bytes, insideBuffer, outsideBuffer);
    size_t downSample_len_samples = Goertzel::downSample32bitBy4((int32_t*)insideBuffer, mono_len_bytes / bytesPerSample);
    // mono_len in bytes
    if (fileInside.write((uint8_t*)insideBuffer, downSample_len_samples * bytesPerSample) != (downSample_len_samples * bytesPerSample)) {
      while (1) {
        Serial.println("Inside writeBlocks Error");  // stopping here so just print unbuffered
        delay(5000);
      }
    }
    downSample_len_samples = Goertzel::downSample32bitBy4((int32_t*)outsideBuffer, mono_len_bytes / bytesPerSample);
    if (fileOutside.write((uint8_t*)outsideBuffer, downSample_len_samples * bytesPerSample) != (downSample_len_samples * bytesPerSample)) {
      while (1) {
        Serial.println("Outside writeBlocks Error");  // stopping here so just print unbuffered
        delay(5000);
      }
    }
    buffer_counter++;
    read_size_2 = 0;
    read_2_Processed = true;
  }
}