#include <SPI.h>
#include <SD.h>
#include <CSV_Parser.h>
#include <stdio.h>

File file;
const int CS_PIN = 5;
const char filepath[] = "/sample_text.csv";
const float RSSI_TO_DIST_N = 1.96f;
const float RSSI_TO_DIST_T = -41.6f;

constexpr int NUM_ROWS = 3;
constexpr int NUM_COLS = 20;

void WriteTestData(){
  WriteFile(filepath, "-45,-45,-47,-45,-45,-46,-46,-46,-45,-45,-44,-46,-46,-47,-47,-47,-47,-46,-48,-47,\n"
  "-53,-51,-49,-53,-53,-49,-54,-49,-50,-50,-53,-54,-51,-53,-54,-50,-52,-50,-51,-53,\n"
  "-67,-67,-64,-64,-63,-65,-62,-60,-58,-58,-63,-67,-67,-58,-62,-66,-66,-64,-62,-62,\n");
}

void WriteFile(const char * path, const char * message){
  file = SD.open(path, FILE_WRITE);
  if (file) {
    file.print(message);
    file.close();
  } 
  else {
    Serial.print("Error opening file: ");
    Serial.println(path);
  }
}

void ReadFile(const char * path, int8_t outputBuffer[3][20]){
  const int rows = 3;
  const int cols = 20;
  int8_t data[rows][cols];
  file = SD.open(path);
  if (file) {
    for(int row=0; row < rows; row++){
      for(int col=0; col < cols; col++){
        String val = file.readStringUntil(',');
        data[row][col] = (int8_t) val.toInt();
        outputBuffer[row][col] = data[row][col];
      }
      //  memcpy(&outputBuffer[row][0], &data[row][0], sizeof(int8_t) * cols);
    }
    file.close(); 
  } else {
    Serial.println("error opening test.txt");
  }

  Serial.println("Parsed data in file:");
  for(int row=0; row < rows; row++){
    for(int col=0; col < cols; col++){
      Serial.printf("%d ", data[row][col]);
    }
    Serial.println("");
  }
}

// This function converts an rssi measurement to a distance measurement
float RSSI2Dist(float rssi) {
  return powf(10.0, (RSSI_TO_DIST_T - rssi)/(10.0*RSSI_TO_DIST_N));
}


void setup() {
  Serial.begin(9600);
  delay(500);
  while (!Serial) { ; }
  Serial.println("Serial Initialized...");
  if (!SD.begin(CS_PIN)) {
    Serial.println("SD card initialization failed!");
    return;
  }
  Serial.println("SD card initialization done.");

  // Step 0: Explain what the code is doing so far
  // Step 1: Fix the code so that we are able to write and read from the SD card
  WriteTestData();

  // Step 2: Modify ReadFile function so that it returns the parsed data. Hint: first explain how you 
  // would return it if it was a 1D array, then ask yourself how you could treat it like a 1D array
  int8_t outputData[NUM_ROWS][NUM_COLS] = { 0 };

  Serial.println("Copied Buffer: ");

  ReadFile(filepath, outputData);


  float sum;

  for(int row=0; row < NUM_ROWS; row++){
    sum = 0;
    for(int col=0; col < NUM_COLS; col++){
      sum += outputData[row][col];
    }
    Serial.printf("%f\n", sum / NUM_COLS);
  }
  

  // Step 3: Calculate the mean distance of each row of data
  // Note: you should the mean rssi first, and then compute the distance
  // Correct Solution: [1.6768, 3.2375, 12.7231]
}

void loop() {
  // nothing happens after setup
}