#if !defined(ARDUINO_ARCH_RP2040)
  #error For RP2040 only
#endif

#if defined(ARDUINO_ARCH_MBED)
  
  #define PIN_SD_MOSI       PIN_SPI_MOSI
  #define PIN_SD_MISO       PIN_SPI_MISO
  #define PIN_SD_SCK        PIN_SPI_SCK
  #define PIN_SD_SS         PIN_SPI_SS

#else

  #define PIN_SD_MOSI       PIN_SPI0_MOSI
  #define PIN_SD_MISO       PIN_SPI0_MISO
  #define PIN_SD_SCK        PIN_SPI0_SCK
  #define PIN_SD_SS         PIN_SPI0_SS
  
#endif

#define _RP2040_SD_LOGLEVEL_       0

#include <SPI.h>
#include <RP2040_SD.h>

int DEBUG = 1;  // Set DEBUG mode to 1 for printing, 0 to disable printing
int smoothing = 1;
int msResolution = 10;
int tps;
float afr;
float lambda;
char logfile[13];
const int rpmPin = 21; 
const int afrPin = 26; 
const int tpsPin = 27; 
const int sensorInterrupt = 0; 
const int timeoutValue = 10; 
volatile unsigned long lastPulseTime; 
volatile unsigned long interval = 0; 
volatile int timeoutCounter; 
long rpm;                                             
long rpm_last;     
int cal = 10; 
int justfixed; 
char recentDatePrefix[] = "1742";
unsigned long recentDate = 673975746;

// Buffers for averaging and filtering
const int numReadings = 5;
int rpmarray[numReadings];
int rpmindex = 0;                  // the index of the current reading
long total = 0;                  // the running total
long average = 0;                // the average

const int tpsNumReadings = 15;
int tpsarray[tpsNumReadings];
int tpsindex = 0;                  // the index of the current reading
long tpstotal = 0;                  // the running total
long tpsaverage = 0; 

const int afrNumReadings = 10;
int afrarray[afrNumReadings];
int afrindex = 0;                  // the index of the current reading
float afrtotal = 0;                  // the running total
float afraverage = 0; 

char filename[16];

// Buffer to hold data entries before writing to SD card
char dataBuffer[512]; // Buffer for storing data entries
int bufferIndex = 0;  // Index for the data buffer

File dataFile;

void setup() {
  if (DEBUG) {
    Serial.begin(250000);
  }

  pinMode(rpmPin, INPUT_PULLUP);
  pinMode(afrPin, INPUT_PULLUP);
  pinMode(tpsPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(rpmPin), sensorIsr, RISING); 
  analogReadResolution(12);

  if (DEBUG) {
    #if defined(ARDUINO_ARCH_MBED)
      Serial.print("Starting SD Card ReadWrite on MBED ");
    #else
      Serial.print("Starting SD Card ReadWrite on ");
    #endif
    
    Serial.println(BOARD_NAME);
    Serial.println(RP2040_SD_VERSION);
    
    Serial.print("Initializing SD card with SS = ");  Serial.println(PIN_SD_SS);
    Serial.print("SCK = ");   Serial.println(PIN_SD_SCK);
    Serial.print("MOSI = ");  Serial.println(PIN_SD_MOSI);
    Serial.print("MISO = ");  Serial.println(PIN_SD_MISO);
  }

  if (!SD.begin(PIN_SD_SS)) {
    if (DEBUG) { 
      Serial.println("Initialization failed!");
    }
    return;
  }
  
  if (DEBUG) {
    Serial.println("Initialization done.");
  }

  // Open file once in setup
  int n = 0;
  snprintf(filename, sizeof(filename), "data%03d.csv", n); // includes a three-digit sequence number in the file name
  while(SD.exists(filename)) {
    n++;
    snprintf(filename, sizeof(filename), "data%03d.csv", n);
  }

  dataFile = SD.open(filename, FILE_WRITE);
  if (!dataFile) {
    Serial.println("Error opening file!");
    return;
  }

  // Write headers to the file
  dataFile.println("timestamp,rpm,tps,afr,lambda");
  dataFile.close();
  if (DEBUG) {
    Serial.print("File: "); 
    Serial.println(filename);
  }
}

void sensorIsr() { 
  unsigned long now = micros(); 
  interval = now - lastPulseTime; 
  lastPulseTime = now; 
  timeoutCounter = timeoutValue; 
} 

char* getTimeStamp() {
  static char timestamp[14];  // Static buffer to store the formatted timestamp
  snprintf(timestamp, sizeof(timestamp), "%s%lu", recentDatePrefix, recentDate + millis());  // Format the timestamp
  return timestamp;  // Return the pointer to the formatted timestamp
}
void writeDataToCSV(char* data) {
  // Append data to the buffer
  int len = strlen(data);
  if (bufferIndex + len < sizeof(dataBuffer)) {
    memcpy(dataBuffer + bufferIndex, data, len);
    bufferIndex += len;
  }

  // If buffer is full, write it to SD
  if (bufferIndex >= sizeof(dataBuffer) - 100) {  // Reserve space for new entry
    dataFile = SD.open(filename, FILE_WRITE);
    dataFile.print(dataBuffer);   // Write the buffer to the SD card
    dataFile.close();
    bufferIndex = 0;             // Reset buffer index
  }
}

void loop() {
  // Read sensor values
  afr = (float)map(analogRead(afrPin), 0, 3103, 100, 200) / 10;
  lambda = (float)map(analogRead(afrPin), 0, 3103, 68, 136) / 100;   
  tps = map(analogRead(tpsPin), 780, 2590, 0, 100);
  
  if (tps <= 0) {
    tps = 0;
  }
  if (tps >= 100) {
    tps = 100;
  }

  rpm = long(60e7 / cal) / (float)interval;

  if (timeoutCounter > 0) { timeoutCounter--; }  
  if (timeoutCounter <= 0) { rpm = 0; }

    if (((rpm > (rpm_last +(rpm_last*.2))) || (rpm < (rpm_last - (rpm_last*.2)))) && (rpm_last > 0) && (justfixed < 3)){
        rpm = rpm_last;
        justfixed++;
  } else {
    rpm_last = rpm;
    justfixed--;
    if (justfixed <= 0){justfixed = 0;}
  }

  // RPM smoothing logic
  if (smoothing) {
    total = total - rpmarray[rpmindex]; 
    rpmarray[rpmindex] = rpm;
    total = total + rpmarray[rpmindex]; 
    rpmindex = rpmindex + 1;   
    if (rpmindex >= numReadings) { rpmindex = 0; }                       
    average = total / numReadings;  
    rpm = average;       
  }

  // TPS smoothing logic
  if (smoothing) {
    tpstotal = tpstotal - tpsarray[tpsindex]; 
    tpsarray[tpsindex] = tps;
    tpstotal = tpstotal + tpsarray[tpsindex]; 
    tpsindex = tpsindex + 1;   
    if (tpsindex >= tpsNumReadings) { tpsindex = 0; }                       
    tpsaverage = tpstotal / tpsNumReadings;  
    tps = tpsaverage;       
  }

  // AFR smoothing logic
  if (smoothing) {
    afrtotal = afrtotal - afrarray[afrindex]; 
    afrarray[afrindex] = afr;
    afrtotal = afrtotal + afrarray[afrindex]; 
    afrindex = afrindex + 1;   
    if (afrindex >= afrNumReadings) { afrindex = 0; }                       
    afraverage = afrtotal / afrNumReadings;  
    afr = afraverage;       
  }

  // Prepare the data string to append to the buffer
  char data[40];
  snprintf(data, sizeof(data), "%s,%ld,%d,%.2f,%.2f\n", getTimeStamp(), rpm, tps, afr, lambda);

  // Write data to SD (in buffer)
  writeDataToCSV(data);

  if (DEBUG) {
    Serial.print(analogRead(tpsPin));
    Serial.print(" ");
    Serial.print(analogRead(afrPin));
    Serial.print(" ");
    Serial.print(data);  
  }
  delay(5);
}