#include <SD.h>
#include <SPI.h>
#include <avr/wdt.h>
#define BLINK_RATE 200


//=== Objects ===
File logFile;


//=== State ===
bool SDnotFound = true;

// unsigned long previousTime = 0;


void blinkIfAlive() {
  static uint32_t nextBlink = 0;
  if (millis() > nextBlink) {
    digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); // pinMode(LED_BUILTIN, OUTPUT) in setup()
    nextBlink += BLINK_RATE;  // #define BLINK_RATE 200 or whatever
  }
}


void checkSerial() {
  if (Serial.available()) {
    uint8_t cmd = Serial.read();
    if(cmd == 'r') {
      readSDCard();
    }
  }
}


void SDsetup() {
  //Iniitialization
  Serial.println(F("Inizializzazione SD..."));
  if (!SD.begin(10)) {
    Serial.println(F("Initialising SD failed!"));
    SDnotFound = true;
    while (1);
  }
  Serial.println(F("SD initialized correctly"));
  SDnotFound = false;

  //Opening file and writing header
  logFile = SD.open("log.csv", FILE_WRITE);
  if (logFile) {
    logFile.println(F("Stinger logging system online"));
    logFile.println();
    logFile.println(F("Time,AX,AY,AZ,GX,GY,GZ,GXF,GYF,GZF,OUTX,OUTY,OUTZ,ANGX,ANGY,ANGZ,S1,S2,S3,S4,OFFX,OFFY,LAUNCHED,MODE"));
    logFile.flush(); //Ensure data is written to SD card
  } else {
    Serial.println("Can't open SD File");
  }
}

void readSDCard() {
  // open the file for reading:
  logFile = SD.open("log.csv");
  if (logFile) {
    Serial.println("log.csv:");

    // read from the file until there's nothing else in it:
    while (logFile.available()) {
      Serial.write(logFile.read());
    }
    // close the file:
    logFile.close();
  } else {
    // if the file didn't open, print an error:
    Serial.println("error opening test.txt");
  }
}
//=== Setup ===
void setup() {
  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(115200);
  SDsetup();
  Serial.print("Initialisation done in ");
  Serial.print(millis());
  Serial.println(" ms");
}


//=== Main loop ===
void loop() {
  blinkIfAlive();
  checkSerial();
}