#ifndef _LOGGING_H_
#define _LOGGING_H_

// SD Card Logging

#include <SPI.h>
#include <SD.h> //Format sd cart with FAT or FAT16. FAT32 for >1GB Cards on Teensy4.1

//#define SDCHIPSELECT 14
boolean sdcard_available=false;
boolean datalogging=true;
String datalogging_filename="UNKNOWN.txt";

uint16_t chunksize=128; //for bulk data transmission

#define LOGGINGINTERVAL 100 //logging interval when driving
#define LOGGINGSTANDSTILLTIME 30000 //after which time of standstill logging interval should change
#define LOGGINGINTERVALSTANDSTILL 500 //logging interval when standing still
#define LOGGINGINTERVALDISARMED 1000 //logging interval when disarmed

bool serialCommandEcho_Enabled=true;

bool initLogging();
void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear);


void writeLogComment(unsigned long time, String msg);
void printFileListing(bool pretty);
void printDirectory(File dir, int numTabs,String parent,bool printSize,bool printFullDirectoryNames);
void printFile(String filename);
void printFilesize(String filename);
void transferFile(String filename);
void removeFile(String filename);
void serialCommandLoop(unsigned long loopmillis, ESCSerialComm& escFront, ESCSerialComm& escRear);
float getBatteryVoltage(ESCSerialComm& escFront, ESCSerialComm& escRear);


bool initLogging() {
  Serial.print("Datalogging is ");
  if (datalogging) {
    Serial.println("enabled");
  }else{
    Serial.println("disabled");
  }
  Serial.print("Initializing SD card...");
  // see if the card is present and can be initialized:
  if (!SD.begin(BUILTIN_SDCARD)) {
      Serial.println("Card failed, or not present");
      display.println(F("SD Init Fail!")); display.display();
      sdcard_available=false; //disable logging
      datalogging=false;
      delay(1000);
      return false;
  }else{
      sdcard_available=true;
      Serial.println("Card initialized.");
      display.print(F("LOG=")); display.display();
    
      int filenumber=0;
      char buffer[6];
      sprintf(buffer, "%04d", filenumber);
      datalogging_filename="LOG_"+String(buffer)+".TXT";
      while(SD.exists(datalogging_filename.c_str()) && filenumber<10000) {
          //Serial.print(datalogging_filename); Serial.println(" exists");
          filenumber++;
          sprintf(buffer, "%04d", filenumber);
          datalogging_filename="LOG_"+String(buffer)+".TXT";

      }      
      Serial.print(datalogging_filename); Serial.println(" is free");
      display.print(datalogging_filename); 
      if (!datalogging) { //datalogging is disabled during boot
        display.print(" OFF");
      }
      display.println(); display.display();
      
  }
  return true;
}


void loggingLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {

  static unsigned long last_datalogging_write=0;
  static boolean logging_headerWritten=false;

  unsigned long logginginterval=LOGGINGINTERVAL;
  if (armed) { //Change Logginginterval when less is happening
    if (loopmillis-last_notidle>LOGGINGSTANDSTILLTIME) {
      logginginterval=LOGGINGINTERVALSTANDSTILL;
    }
  }else{ //disamred
    logginginterval=LOGGINGINTERVALDISARMED;
  }

  
  if (loopmillis-last_datalogging_write>logginginterval)
  {
    last_datalogging_write=loopmillis;

    if (datalogging && sdcard_available) {

      File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);

      if (dataFile) { // if the file is available, write to it
        if (!logging_headerWritten) {
          dataFile.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
          dataFile.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
          dataFile.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
          dataFile.print("temp_ESCFront,temp_ESCRear,vbat_Front,vbat_Rear,");
          dataFile.print("currentAll,throttle,brake,speed,trip_Front,trip_Rear,trip,currentConsumed_Front,currentConsumed_Rear,currentConsumed,watthoursConsumed,");
          dataFile.println("temp_Front,temp_Rear,temp_Air,looptime_duration_min,looptime_duration_max");
          dataFile.print("#TIMESTAMP:"); dataFile.println(now());
          logging_headerWritten=true;
        }
        dataFile.print(loopmillis/1000.0,3); dataFile.print(",");
        dataFile.print(escFront.getCmdL());  dataFile.print(",");
        dataFile.print(escFront.getCmdR());  dataFile.print(",");
        dataFile.print(escRear.getCmdL());  dataFile.print(",");
        dataFile.print(escRear.getCmdR());  dataFile.print(",");
        dataFile.print(escFront.getFiltered_curL(),3);  dataFile.print(",");
        dataFile.print(escFront.getFiltered_curR(),3);  dataFile.print(",");
        dataFile.print(escRear.getFiltered_curL(),3);  dataFile.print(",");
        dataFile.print(escRear.getFiltered_curR(),3);  dataFile.print(",");
        dataFile.print(escFront.getFeedback_speedL_meas());  dataFile.print(","); //+ //Todo: check if speed for R wheels needs to be negated
        dataFile.print(escFront.getFeedback_speedR_meas());  dataFile.print(","); //-
        dataFile.print(escRear.getFeedback_speedL_meas());  dataFile.print(","); //+
        dataFile.print(escRear.getFeedback_speedR_meas());  dataFile.print(","); //-
        dataFile.print(escFront.getFeedback_boardTemp());  dataFile.print(",");
        dataFile.print(escRear.getFeedback_boardTemp());  dataFile.print(",");
        dataFile.print(escFront.getFeedback_batVoltage());  dataFile.print(",");
        dataFile.print(escRear.getFeedback_batVoltage());  dataFile.print(",");
        dataFile.print(filtered_currentAll,3);  dataFile.print(",");
        dataFile.print(throttle_pos);  dataFile.print(",");
        dataFile.print(brake_pos);  dataFile.print(",");
        dataFile.print((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);  dataFile.print(",");
        dataFile.print(escFront.getTrip());  dataFile.print(",");
        dataFile.print(escRear.getTrip());  dataFile.print(",");
        dataFile.print(trip);  dataFile.print(",");
        dataFile.print(escFront.getCurrentConsumed(),3); dataFile.print(",");
        dataFile.print(escRear.getCurrentConsumed(),3); dataFile.print(",");
        dataFile.print(currentConsumed,3); dataFile.print(",");
        dataFile.print(watthoursConsumed,3); dataFile.print(",");
        dataFile.print(temp_Front,2); dataFile.print(",");
        dataFile.print(temp_Rear,2); dataFile.print(",");
        dataFile.print(temp_Air,2); dataFile.print(",");
        dataFile.print(looptime_duration_min); dataFile.print(",");
        dataFile.print(looptime_duration_max); //dataFile.print(",");
        looptime_duration_max=0; //reset
        looptime_duration_min=1000; //reset
        
        
        
        dataFile.println("");
        dataFile.close();
      }else{ //dataFile not available
        error_sdfile_unavailable=true;
      }
    }

    looptime_duration_max=0; //reset
    looptime_duration_min=1000; //reset

  }
}

void writeLogComment(unsigned long time, String msg) {
    //SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
    if (datalogging && sdcard_available) {
        File dataFile = SD.open(datalogging_filename.c_str(), FILE_WRITE);

        if (dataFile) { // if the file is available, write to it
            dataFile.print("#");
            dataFile.print(time/1000.0,3); 
            dataFile.print(","); 
            dataFile.print(msg); 
            dataFile.println();
            dataFile.close();
        }
    }
}

String getLogFilename() {
  return datalogging_filename;
}

bool getDatalogging() {
  return datalogging;
}

bool loadTripSD()
{
  //stats.txt containt lines with driven distance or consume current.
  //the lowest line is the latest data
  //data is usually written rather slowly

  File myFile = SD.open("stats.txt", FILE_WRITE);
  if (myFile) {
    myFile.print("ini=");
    myFile.println(now());
    myFile.flush();
    delay(100);
    myFile.close();
    Serial.println("TripSD: time written");
  }else{
    Serial.println("TripSD: could not open stats.txt");
    return false;
  }


  myFile = SD.open("stats.txt");
  if (myFile) {
    while (myFile.available()) {
      String line=myFile.readStringUntil('\n');      
      Serial.print("TripSD Read Line:");
      Serial.println(line);
      if (line.substring(0,4)=="trp="){
        overallTrip=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="occ="){
        overallCurrentConsumed=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="owh="){
        overallWatthoursConsumed=(line.substring(4)).toFloat();
      }else if (line.substring(0,4)=="vlt="){
        lastTripVoltage=(line.substring(4)).toFloat();
      }
    }
    Serial.print("TripSD trip:");
    Serial.println(overallTrip);
    display.print(F("TripSD=")); display.println(overallTrip);
    Serial.print("TripSD current:");
    Serial.println(overallCurrentConsumed);
    Serial.print("TripSD watthours:");
    Serial.println(overallWatthoursConsumed);
    display.print(F("Cur. SD=")); display.println(overallCurrentConsumed); display.display();
    Serial.print("TripSD voltage:");
    Serial.println(lastTripVoltage);
    display.print(F("Volt. SD=")); display.println(lastTripVoltage); display.display();

    myFile.flush();
    delay(10);
    myFile.close();
  }else{
    return false;
  }
  return true;
}

void writeTrip(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
  if (datalogging && sdcard_available) {
    File myFile = SD.open("stats.txt", FILE_WRITE);
    if (myFile) {
      myFile.print("trp=");
      myFile.println(overallTrip);
      myFile.print("occ=");
      myFile.println(overallCurrentConsumed);
      myFile.print("owh=");
      myFile.println(overallWatthoursConsumed);
      myFile.print("vlt=");
      float _voltage=getBatteryVoltage(escFront, escRear);
      myFile.println(_voltage,2);
      myFile.close();
    }
  }
}

float getBatteryVoltage(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  return (escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0;
}

void resetTrip() {
  if (datalogging && sdcard_available) {
    File myFile = SD.open("stats.txt", FILE_WRITE);
    if (myFile) {
      myFile.print("trp=");
      myFile.println(0);
      myFile.print("occ=");
      myFile.println(0);
      myFile.print("owh=");
      myFile.println(0);
      myFile.print("vlt=");
      myFile.println(0);
      myFile.flush();
      delay(10);
      myFile.close();
    }
  }
}




void serialCommandLoop(unsigned long loopmillis,ESCSerialComm& escFront, ESCSerialComm& escRear) {
  #define MAX_MESSAGE_LENGTH 80

  uint8_t timeoutcounter=MAX_MESSAGE_LENGTH;
  
  while (Serial.available() > 0 && timeoutcounter>0)
  {
    timeoutcounter--;
    
    static char message[MAX_MESSAGE_LENGTH];
    static unsigned int message_pos = 0;
    
    char inByte = Serial.read();
    if (inByte == '\r') {
      break; //ignore CR
    }
    
    
    if ( inByte != '\n' && (message_pos < MAX_MESSAGE_LENGTH - 1) )
    {
      
      message[message_pos] = inByte;
      message_pos++;
    }
    
    else
    {
      message[message_pos] = '\0';
      
      
      String smessage=String(message);

      if(smessage.equals("echo off")) {
        serialCommandEcho_Enabled=false;
      }else if(smessage.equals("echo on")) {
        serialCommandEcho_Enabled=true;
      }

      if (serialCommandEcho_Enabled) { //Echo Command
        Serial.print("$"); Serial.println(message);
      }

      //Handle Commands
      if(smessage.equals("test")) {
        Serial.println("OK");
      }else if(smessage.equals("ls")) {
        printFileListing(false);
      }else if(smessage.equals("ls -a")) {
        printFileListing(true);
      }else if(smessage.startsWith("cat")) {
        String _filename=smessage.substring(4);
        if (_filename.length()<3){ //if no filename given, use current log file
          _filename=getLogFilename();
        }
        printFile(_filename);
      }else if(smessage.startsWith("get")) {
        String _filename=smessage.substring(4);
        transferFile(_filename);
      }else if(smessage.startsWith("sizeof")) {
        String _filename=smessage.substring(7);
        printFilesize(_filename);
      }else if(smessage.startsWith("chunksize")) { //change chunksize for get command
        String _chunksizestring=smessage.substring(10);
        long _chunksize=_chunksizestring.toInt();        
        if (_chunksize>0 && _chunksize<LONG_MAX) { //value ok
          chunksize=_chunksize;
        }else{
          Serial.println("Chunksize value out of range");
        }
      }else if(smessage.startsWith("rm")) {
        removeFile(smessage.substring(3));
      }else if(smessage.equals("log off")) {
        writeLogComment(loopmillis, "Datalogging disabled by serial command");  
        datalogging=false;
        Serial.print("Log disabled: "); Serial.println(datalogging_filename);
      }else if(smessage.equals("log on")) {
        datalogging=true;
        writeLogComment(loopmillis, "Datalogging enabled by serial command");  
        Serial.print("Log enabled: "); Serial.println(datalogging_filename);
      }else if(smessage.equals("show stats")) {
        Serial.print("overallTrip\t"); Serial.println(overallTrip);
        Serial.print("overallCurrentConsumed\t"); Serial.println(overallCurrentConsumed);
        Serial.print("overallWatthoursConsumed\t"); Serial.println(overallWatthoursConsumed);
        Serial.print("voltage\t"); Serial.println(getBatteryVoltage(escFront,escRear));
      }else if(smessage.equals("show sensors")) {
        Serial.print("loopmillis\t"); Serial.println(loopmillis);
        Serial.print("temp_ESCFront\t"); Serial.println(escFront.getFeedback_boardTemp());
        Serial.print("temp_ESCRear\t"); Serial.println(escRear.getFeedback_boardTemp());
        Serial.print("vbat_Front\t"); Serial.println(escFront.getFeedback_batVoltage());
        Serial.print("vbat_Rear\t"); Serial.println(escRear.getFeedback_batVoltage());        
        Serial.print("throttle\t"); Serial.println(throttle_pos);
        Serial.print("brake\t"); Serial.println(brake_pos);
        Serial.print("trip_Front\t"); Serial.println(escFront.getTrip());
        Serial.print("trip_Rear\t"); Serial.println(escRear.getTrip());
        Serial.print("trip\t"); Serial.println(trip);
        Serial.print("currentConsumed_Front\t"); Serial.println(escFront.getCurrentConsumed(),3);
        Serial.print("currentConsumed_Rear\t"); Serial.println(escRear.getCurrentConsumed(),3);
        Serial.print("currentConsumed\t"); Serial.println(currentConsumed,3);
        Serial.print("watthoursConsumed\t"); Serial.println(watthoursConsumed,3);
        Serial.print("temp_Front\t"); Serial.println(temp_Front,3);
        Serial.print("temp_Rear\t"); Serial.println(temp_Rear,3);
        Serial.print("temp_Air\t"); Serial.println(temp_Air,3);
        Serial.print("looptime_duration_min\t"); Serial.println(looptime_duration_min);
        Serial.print("looptime_duration_max\t"); Serial.println(looptime_duration_max);
      }
      
      message_pos = 0;
    }
  }
}

void printFileListing(bool pretty) {
  File root;
  root = SD.open("/");

  if (pretty) {
    printDirectory(root, 0,"",true,false); //tabbed with file sizes
  }else{
    printDirectory(root, 0,"",false,true); //full paths without size
  }
}


void printDirectory(File dir, int numTabs,String parent,bool printSize,bool printFullDirectoryNames) {
  

  while (true) {

    File entry =  dir.openNextFile();
    if (! entry) {
      // no more files
      break;
    }
    if (!printFullDirectoryNames) {
      for (uint8_t i = 0; i < numTabs; i++) {
        Serial.print('\t');
      }
      
    }

    if (!entry.isDirectory() || !printFullDirectoryNames) {
      Serial.print(parent+entry.name());
    }
    
    if (entry.isDirectory()) {
      if (!printFullDirectoryNames) {
        Serial.println("/");
      }
      printDirectory(entry, numTabs + 1,parent+entry.name()+"/",printSize,printFullDirectoryNames);
    } else {
      if (printSize) {
        // files have sizes, directories do not
        Serial.print("\t\t");
        Serial.print(entry.size(), DEC);
      }
      Serial.println();
    }
    entry.close();
  }
}

void printFilesize(String filename) {
  File dataFile = SD.open(filename.c_str(), FILE_READ);
  if (dataFile) {
    Serial.println(dataFile.size(), DEC);
    dataFile.close();
  }else{
    Serial.println('0');
  }

}


void printFile(String filename) {

  File dataFile = SD.open(filename.c_str(), FILE_READ);

  

  // if the file is available, write to it:
  if (dataFile) {
    while (dataFile.available()) {
      Serial.write(dataFile.read());
    }
    dataFile.close();
  }
  // if the file isn't open, pop up an error:
  else {
    Serial.print("error opening "); Serial.println(filename);
  }
  
}


void transferFile(String filename) {
  //similar to printFile, but with checksum and chunk based transfer
  
  
  

  File dataFile = SD.open(filename.c_str(), FILE_READ);

  bool transmiterror=false;

  // if the file is available, write to it:
  if (dataFile) {
    while (dataFile.available() && !transmiterror) {
      byte checksum=0;
      for (uint16_t i=0;i<chunksize;i++) {
        if (dataFile.available()) {
          byte b = dataFile.read();
          Serial.write(b);
          checksum+=b;
        }
      }
      int inByte=-1;
      while (inByte==-1) {
        if (Serial.available() > 0)
        {
          inByte = Serial.read();
          
          if ((byte)inByte!=checksum){
            transmiterror=true; //exit
            Serial.println();
            Serial.println("TRANSMISSION ERROR WRONG CHECKSUM");
          }
        }
      }

    }
    dataFile.close();
  }
  // if the file isn't open, pop up an error:
  else {
    Serial.print("error opening "); Serial.println(filename);
  }
  
}


void removeFile(String filename) {

  SD.remove(filename.c_str());
  if (SD.exists(filename.c_str())) {
    Serial.println("File still exists");
  } else {
    Serial.println("OK");
  }

}



#endif