#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <TFT_eSPI.h> // Graphics and font library for ILI9341 driver 
#include"LIS3DHTR.h"
#include "RTC_SAMD51.h"
#include "DateTime.h"
#include <SPI.h>
#include <SD.h>
#include <FlashStorage_SAMD.h>
#define FLASH_DEBUG       0
#define TFT_GREY  0x5AEB
#define YELLOW    0xFFE0
#define RED       0xF800
FlashStorage(my_flash_store, int);
TFT_eSPI tft = TFT_eSPI();  // Invoke library
LIS3DHTR<TwoWire> lis;
RTC_SAMD51 rtc;

int captureTime;
int SDWriteTime;
unsigned long avSDWriteTime =0;
int SDwriteCycleTime;
unsigned long start1;
unsigned long finish1;
unsigned long start2;
unsigned long finish2;
unsigned long currentMicros1;
unsigned long previousMicros1 =0;
unsigned long count2 = 0;
// string to buffer output
String buffer;
String filename;
unsigned long lastMillis = 0;
int stateOne = 0;
float flat = 0.00000000;
float flon = 0.00000000;
float GPSspeed = 0.0;
int GPSsatelites = 0;
int GPSprecision = 0;
int flashNumber;
uint16_t address = 0;

// File object to represent file
File txtFile;

/* This sample code demonstrates the normal use of a TinyGPS object.
   It requires the use of SoftwareSerial, and assumes that you have a
   9600-baud serial GPS device hooked up on pins D0(rx) and D1(tx) - the
   SIM28 Seed studio grove GPS module.
   
*/

TinyGPS gps;
SoftwareSerial ss(0, 1);

void setup()
{
 // Read the content of emulated-EEPROM
  EEPROM.get(address, flashNumber);
  // Save into emulated-EEPROM the number increased by 1 for the next run of the sketch
  EEPROM.put(address, (int) (flashNumber + 1));
  if (!EEPROM.getCommitASAP())
  {
    // Serial.println("CommitASAP not set. Need commit()");
    EEPROM.commit();
  }
  tft.init();
  tft.setRotation(2);
  tft.fillScreen(TFT_BLACK);
  tft.setCursor(0, 0, 2);
  tft.setTextColor(YELLOW, TFT_BLACK);  tft.setTextSize(2);
  tft.println(" GPS + G-Force ");
  tft.println("  Data Logger  ");
  tft.println("     V.1.01    ");
  tft.setTextColor(TFT_WHITE, TFT_BLACK);  tft.setTextSize(2);
 
  Serial.begin(115200);
  // while(!Serial){1;};
  // tft.println("Serial started ...");
  delay(5000);
  ss.begin(9600);
  lis.begin(Wire1);
  rtc.begin();
  
  DateTime now = DateTime(F(__DATE__), F(__TIME__));
  Serial.println("adjust time!");
  rtc.adjust(now);

  // yyyy/MM/dd HH:mm:ss
  // SD file name is restricted to 8 characters !!
  // filename += now.year(); filename += "";
  // filename += now.month(); filename += "";
  filename += now.day(); filename += "";
  filename += now.hour(); filename += "_";
  // Use a flash stored variable to ensure unique filename:
  filename += String(flashNumber);
  filename += ".txt";

  if (!lis) 
  {
    Serial.println("ERROR");
    while(1);
  }
  lis.setOutputDataRate(LIS3DHTR_DATARATE_50HZ); //Data output rate
  lis.setFullScaleRange(LIS3DHTR_RANGE_2G); //Scale range set to 2g

  // reserve 1kB for String used as a buffer
  buffer.reserve(2048);

  // set LED pin to output, used to blink when writing
  pinMode(LED_BUILTIN, OUTPUT);

  // init the SD card
  if (!SD.begin()) 
  {
    tft.setTextColor(RED, TFT_BLACK);  tft.setTextSize(2);
    tft.println("  No SD card?  ");
    Serial.println("Card failed, or not present");
    // don't do anything more:
    while (1);
  }

  // If you want to start from an empty file,
  // uncomment the next line:
  // SD.remove(filename);

  // try to open the file for writing
  txtFile = SD.open(filename, FILE_WRITE);
  if (!txtFile) 
  {
    Serial.print("error opening ");
    Serial.println(filename);
    while (1);
  }

  txtFile.write("Millis,Timestamp,Accel_X,Accel_Y,Accel_Z,Satelites,Speed(km/h),Latitude,Longitude,GPSprecision \r\n");
  
  Serial.print("Simple TinyGPS library v. "); Serial.println(TinyGPS::library_version());
  Serial.println("by Mikal Hart");
  Serial.println();
}

void loop()
{
  bool newData = false;
  unsigned long chars;
  unsigned short sentences, failed;

  // For one second we parse GPS data and report some key values
  for (unsigned long currentMillis = millis(); millis() - currentMillis < 1000;)
  {
    while (ss.available())
    {
      start1 = micros();
      char c = ss.read();
      Serial.write(c); // uncomment this line if you want to see the GPS data flowing
      if (gps.encode(c)) // Did a new valid sentence come in?
        newData = true;
      finish1 = micros();
    }
    captureTime = finish1 - start1;

    float x_values, y_values, z_values;
    x_values = lis.getAccelerationX();
    y_values = lis.getAccelerationY();
    z_values = lis.getAccelerationZ();

//////////////////////////////////////////////////////////////////

    // check if it's been over 20 ms since the last line added
    unsigned long currentMillisTwo = millis();
    if ((currentMillisTwo - lastMillis) >= 20)
    {
      DateTime now = rtc.now();
      // Serial.println("Write to the buffer !!");
      // add a new line to the buffer
      buffer += currentMillisTwo; buffer += ",";

      // yyyy/MM/dd HH:mm:ss
      buffer += now.year(); buffer += "-";
      buffer += now.month(); buffer += "-";
      buffer += now.day(); buffer += " ";
      buffer += now.hour(); buffer += ":"; 
      buffer += now.minute(); buffer += ":"; 
      buffer += now.second(); buffer += ",";

      buffer += String(x_values,3); buffer += ",";
      buffer += String(y_values,3); buffer += ",";
      buffer += String(z_values,3); buffer += ",";
      GPSsatelites = gps.satellites();
      if (GPSsatelites == 255)
      {
        GPSsatelites = 0;
      }
      buffer += String(GPSsatelites); buffer += ",";
      GPSspeed = gps.f_speed_kmph();
      if (GPSspeed < 0)
      {
        GPSspeed = 0.0;
      }
      buffer += String(GPSspeed,3); buffer += ",";
      buffer += String(flat,8); buffer += ",";
      buffer += String(flon,8); buffer += ",";
      GPSprecision = gps.hdop();
      if (GPSprecision < 0)
      {
        GPSprecision = 0;
      }
      buffer += String(GPSprecision);
      buffer += "\r\n";
      // Serial.println(flat);
      // Serial.println(buffer);
      lastMillis = currentMillisTwo;
    }

    // check if the SD card is available to write data without blocking
    // and if the buffered data is enough for the full chunk size
    unsigned int chunkSize = txtFile.availableForWrite();
    // Serial.print("chunkSize: "); Serial.print(chunkSize);
    // Serial.print("  buffer.length(): "); Serial.println(buffer.length());
    

    if (chunkSize && buffer.length() >= chunkSize)
    {
      currentMicros1 = micros();
      SDwriteCycleTime = currentMicros1 - previousMicros1;
      previousMicros1 = currentMicros1;
      if (stateOne == 0)
      {
        stateOne = 1;
        digitalWrite(LED_BUILTIN, HIGH);
      }
      else
      {
        stateOne = 0;
        digitalWrite(LED_BUILTIN, LOW);
      }
      // Serial.println("Write the buffer to the SD card !!");
      start2 = micros();
      txtFile.write(buffer.c_str(), chunkSize);
      finish2 = micros();
      SDWriteTime = finish2 - start2;
      
      // Initialise the averaging calc:
      if (avSDWriteTime == 0)
      {
        avSDWriteTime = SDWriteTime;
      }
      // Apply arbitary averaging weights:
      avSDWriteTime = (avSDWriteTime*49 + SDWriteTime)/50;
      
      // remove written data from buffer
      buffer.remove(0, chunkSize);
    }
///////////////////////////////////////////////////////////
  }

  if (newData)
  {
    unsigned long age;
    gps.f_get_position(&flat, &flon, &age);
    Serial.print("LAT=");
    Serial.print(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6);
    Serial.print(" LON=");
    Serial.print(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6);
    Serial.print(" SAT=");
    Serial.print(gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : gps.satellites());
    Serial.print(" PREC=");
    Serial.print(gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : gps.hdop());

    tft.fillScreen(TFT_BLACK);
    tft.setCursor(0, 0, 2);
    tft.print("LAT= ");tft.println(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6);
    tft.print("LON= ");tft.println(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6);
    tft.print("SATS= ");tft.println(gps.satellites() == TinyGPS::GPS_INVALID_SATELLITES ? 0 : gps.satellites());
    tft.print("PREC= ");tft.println(gps.hdop() == TinyGPS::GPS_INVALID_HDOP ? 0 : gps.hdop());
    // tft.print("SPEED= ");tft.println(gps.f_speed_kmph(), TinyGPS::GPS_INVALID_F_SPEED, 6, 2);
    tft.print("SPEED= ");tft.println(gps.f_speed_kmph() == TinyGPS::GPS_INVALID_F_SPEED ? 0.0 : gps.f_speed_kmph());
    tft.print("CAPT= ");tft.print(captureTime);tft.println(" uS");
    tft.print("SDWr= ");tft.print(SDWriteTime);tft.println(" uS");
    tft.print("aSDW= ");tft.print(avSDWriteTime);tft.println(" uS");
    tft.print("SDWc= ");tft.print(SDwriteCycleTime);tft.println(" uS");
    tft.print("FName= ");tft.print(filename);
    
  }
  else
  {
    tft.fillScreen(TFT_BLACK);
    tft.setCursor(0, 0, 2);
    tft.println("No GPS fix");
    tft.print("CAPT= ");tft.println(captureTime);
  }
  
  gps.stats(&chars, &sentences, &failed);
  Serial.print(" CHARS=");
  Serial.print(chars);
  Serial.print(" SENTENCES=");
  Serial.print(sentences);
  Serial.print(" CSUM ERR=");
  Serial.println(failed);
  if (chars == 0)
    Serial.println("** No characters received from GPS: check wiring **");
}