// put your setup code here, to run once:
/*
 * Simple data logger.
 */
#include <SPI.h>
#include <SdFat.h>
#include <Arduino_LSM9DS1.h>
// #include "writeData2Sd.h"
// #include "writeHeader.h"

// SD chip select pin.  Be sure to disable any other SPI devices such as Enet.
// e.g Pin D10 = 10 on Nano BLE33
const uint8_t chipSelect = 10;

// Interval between data records in milliseconds.
// The interval must be greater than the maximum SD write latency plus the
// time to acquire and write data to the SD to avoid overrun errors.
// Run the bench example to check the quality of your SD card.
const uint32_t SAMPLE_INTERVAL_MS = 50;
float xAcc, yAcc, zAcc;

// Log file base name.  Must be six characters or less.
#define FILE_BASE_NAME "Data"
//------------------------------------------------------------------------------
// File system object.
SdFat sd;

// Log file.
SdFile file;

// Time in micros for next data record∏
uint32_t logTime;

//==============================================================================
// User functions.  Edit writeHeader() and logData() for your requirements.

const uint8_t LoggingChannels = 8;  // 4 adc + 3 acc + time

uint8_t bufferLoop = 0;           // variable defining the line in the array of the buffer
const uint8_t bufferLength = 10;  // variable defining the number of lines in buffer -> saving on SD when bufferLoop = bufferLength

float data[LoggingChannels][bufferLength] = {};  // create data array



//------------------------------------------------------------------------------
// ------------      Write data header.    -------------------------------------
//------------------------------------------------------------------------------
void writeHeader() {
  file.print(F("micros"));
  for (uint8_t i = 0; i < 4; i++) {
    file.print(F(",adc"));
    file.print(i, DEC);
  }
  file.print(F(",xAcc"));
  file.print(F(",yAcc"));
  file.print(F(",zAcc"));
  file.println();
}


//------------------------------------------------------------------------------
// ------------       Log a data record.    -------------------------------------
//------------------------------------------------------------------------------
//
void logData(float data[][bufferLength]) {

  //float data[LoggingChannels];
  uint16_t Channel_COUNT = LoggingChannels;
  float xAcc, yAcc, zAcc;


  // Write data to file.  Start with log time in micros.
  data[bufferLoop][0] = (logTime);
  // Read all channels to avoid SD write latency between readings.
  for (uint8_t i = 1; i < Channel_COUNT - 3; i++) {
    data[bufferLoop][i] = analogRead(i);
  }
  IMU.readAcceleration(xAcc, yAcc, zAcc);
  data[bufferLoop][Channel_COUNT - 2] = xAcc;
  data[bufferLoop][Channel_COUNT - 1] = yAcc;
  data[bufferLoop][Channel_COUNT - 0] = zAcc;

  //  Serial.println(logTime);
  // Serial.println(bufferLoop);
    for (uint8_t j = 0; j < bufferLength; j++) {
      for (uint8_t i = 0; i < LoggingChannels; i++) {

      Serial.print(',');
      Serial.print(data[j][i]);
      }
      Serial.println();
    }
  bufferLoop++;


  // // Write ADC data to CSV record.
  // for (uint8_t j = 0; j < bufferLength; j++) {
  //   for (uint8_t i = 0; i < LoggingChannels; i++) {
  //     file.write(',');
  //     file.print(data[j,i]);
  //     Serial.print(',');
  //     Serial.print(data[j,i]);
  //     }
  // Serial.println();
  // file.println();
  // bufferLoop = 0;
  //  }
  // return {data};
  // // Write ADC data to CSV record.
  // for (uint8_t i = 0; i < Channel_COUNT; i++) {
  //   file.write(',');
  //   file.print(data[i]);
  // }
  // file.println();
}




//------------------------------------------------------------------------------
// ------------       write a data record.    -------------------------------------
//------------------------------------------------------------------------------
//
void writeData2Sd(float data[][bufferLength]) {
Serial.println("write Data");
  // Write data to file.  Start with log time in micros.
  // file.print(data[0]);

  // Write ADC data to CSV record.
  for (uint8_t j = 0; j < bufferLength; j++) {
    Serial.println(j);
    for (uint8_t i = 0; i < LoggingChannels; i++) {
      file.print(',');
      file.print(data[j][i]);
      Serial.print(',');
      Serial.print(data[j][i]);
    }
    Serial.println();
    file.println();
    bufferLoop = 0;
  }
}


//==============================================================================
// Error messages stored in flash.
#define error(msg) sd.errorHalt(F(msg))
//------------------------------------------------------------------------------







//========================================================================================
//========================    Setup     ==================================================
//========================================================================================
void setup() {



  const uint8_t BASE_NAME_SIZE = sizeof(FILE_BASE_NAME) - 1;
  char fileName[13] = FILE_BASE_NAME "00.csv";

  Serial.begin(9600);

  // Wait for USB Serial
  while (!Serial) {
    // SysCall::yield();
  }
  delay(1000);

  if (!IMU.begin()) {
    Serial.println("Failed to initialize IMU!");
    while (1)
      ;
  }

  Serial.println(data[0][0], 3);

  Serial.print("Accelerometer sample rate = ");
  Serial.print(IMU.accelerationSampleRate());
  Serial.println(" Hz");
  Serial.println();
  Serial.println("Acceleration in G's");
  Serial.println("X\tY\tZ");


  Serial.println(F("Type any character to start"));
  while (!Serial.available()) {
    // SysCall::yield();
  }

  // Initialize at the highest speed supported by the board that is
  // not over 50 MHz. Try a lower speed if SPI errors occur.
  if (!sd.begin(chipSelect, SD_SCK_MHZ(50))) {
    sd.initErrorHalt();
  }

  // Find an unused file name.
  if (BASE_NAME_SIZE > 6) {
    error("FILE_BASE_NAME too long");
  }
  while (sd.exists(fileName)) {
    if (fileName[BASE_NAME_SIZE + 1] != '9') {
      fileName[BASE_NAME_SIZE + 1]++;
    } else if (fileName[BASE_NAME_SIZE] != '9') {
      fileName[BASE_NAME_SIZE + 1] = '0';
      fileName[BASE_NAME_SIZE]++;
    } else {
      error("Can't create file name");
    }
  }
  if (!file.open(fileName, O_WRONLY | O_CREAT | O_EXCL)) {
    error("file.open");
  }
  // Read any Serial data.
  do {
    delay(10);
  } while (Serial.available() && Serial.read() >= 0);

  Serial.print(F("Logging to: "));
  Serial.println(fileName);
  Serial.println(F("Type any character to stop"));

  // Write data header.
  writeHeader();

  // Start on a multiple of the sample interval.
  logTime = micros() / (1000UL * SAMPLE_INTERVAL_MS) + 1;
  logTime *= 1000UL * SAMPLE_INTERVAL_MS;
}



//========================================================================================
//========================     Loop     ==================================================
//========================================================================================
void loop() {
  // put your main code here, to run repeatedly:
  // Time for next record.
  logTime += 1000UL * SAMPLE_INTERVAL_MS;

  // // Wait for log time.
  // int32_t diff;

  // do {
  //   diff = micros() - logTime;
  // } while (diff < 0);

  // // Check for data rate too high.
  // if (diff > 500) {
  //   Serial.println("missed data record");
  //   //error("Missed data record");
  // }

  if ((micros() - logTime) > 0) {
    logData(data);
    Serial.println("data logged");

    
    if (bufferLoop == bufferLength-1) {
      Serial.println("data writing condition");

      writeData2Sd(data);
    }
  }


  // Force data to SD and update the directory entry to avoid data loss.
  if (!file.sync() || file.getWriteError()) {
    error("write error");
  }

  if (Serial.available()) {
    // Close file and stop.
    file.close();
    Serial.println(F("Done"));
    // SysCall::halt(); // was ist der SysCall??
  }
}