#undef DEBUG_GPS

#include "M5Atom.h"
#include "GPSAnalyse.h"
#include <SPI.h>
#include "FS.h"
#include <SD.h>
#include <Metro.h>

GPSAnalyse GPS;
Metro banggps(2000);
Metro bangimu(20);
Metro banglog(1000);

const char outfilename[] = "/wave-sample.csv";
File of;

typedef struct sGps {
  float lat;
  float lon;
  String utc;
} tGps;

typedef struct sImu {
  // accelerometer
  float aX = 0.0f;
  float aY = 0.0f;
  float aZ = 0.0f;
  
  // gyroscope
  float gX = 0.0f;
  float gY = 0.0f;
  float gZ = 0.0f;
  
  // angles
  float pitch = 0.0f;
  float roll  = 0.0f;
  float yaw   = 0.0f;
  
  float temp = 0.0f;
} tImu;

unsigned long mselapsed;
tGps gpssample;
tImu imusample;

bool writeSample(String filename) {
  of = SD.open(filename, FILE_APPEND);
  if(of) {
    of.print( mselapsed );
    of.print(", ");
    of.print(gpssample.lat);
    of.print(", ");
    of.print(gpssample.lon);
    of.print(", ");
    of.print(gpssample.utc);
    of.print(", ");
    of.print(imusample.aX);
    of.print(", ");
    of.print(imusample.aY);
    of.print(", ");
    of.print(imusample.aZ);
    of.print(", ");
    of.print(imusample.gX);
    of.print(", ");
    of.print(imusample.gY);
    of.print(", ");
    of.print(imusample.gZ);
    of.print(", ");
    of.print(imusample.pitch);
    of.print(", ");
    of.print(imusample.roll);
    of.print(", ");
    of.print(imusample.yaw);
    of.print(", ");
    of.print(imusample.temp);
    of.println(); // EOL
    of.close();
  } else {
    return false;
  }
  return true;
}

void setup(){
  M5.begin(true, false, true);

  M5.IMU.Init(); 

  SPI.begin(23,33,19,-1);
  if(!SD.begin(-1, SPI, 40000000)){
    Serial.println("initialization failed!");
  } 
  sdcard_type_t Type = SD.cardType();

  Serial.printf("SDCard Type = %d \r\n",Type);
  Serial.printf("SDCard Size = %d \r\n" , (int)(SD.cardSize()/1024/1024));

  Serial1.begin(9600,SERIAL_8N1,22,-1);
  GPS.setTaskName("GPS");
  GPS.setTaskPriority(2);
  GPS.setSerialPtr(Serial1);
  GPS.start();

  M5.dis.fillpix(0x000000);
}

void gps_loop() {
  GPS.upDate();
  gpssample.lat = GPS.s_GNRMC.Latitude;
  gpssample.lon = GPS.s_GNRMC.Longitude;
  gpssample.utc = GPS.s_GNRMC.Utc;
}

void imu_loop() {
  //Stores the triaxial gyroscope data of the inertial sensor to the relevant variable
  M5.IMU.getGyroData(&imusample.gX,&imusample.gY,&imusample.gZ);
  M5.IMU.getAccelData(&imusample.aX,&imusample.aY,&imusample.aZ); // accelero
  M5.IMU.getAhrsData(&imusample.pitch,&imusample.roll,&imusample.yaw);  // attitude
  M5.IMU.getTempData(&imusample.temp);  // temperature
}

void debugSample() {
    Serial.print( mselapsed );
    Serial.print(", ");
    Serial.print(gpssample.lat);
    Serial.print(", ");
    Serial.print(gpssample.lon);
    Serial.print(", ");
    Serial.print(gpssample.utc);
    Serial.print(", ");
    Serial.print(imusample.aX);
    Serial.print(", ");
    Serial.print(imusample.aY);
    Serial.print(", ");
    Serial.print(imusample.aZ);
    Serial.print(", ");
    Serial.print(imusample.gX);
    Serial.print(", ");
    Serial.print(imusample.gY);
    Serial.print(", ");
    Serial.print(imusample.gZ);
    Serial.print(", ");
    Serial.print(imusample.pitch);
    Serial.print(", ");
    Serial.print(imusample.roll);
    Serial.print(", ");
    Serial.print(imusample.yaw);
    Serial.print(", ");
    Serial.print(imusample.temp);
    Serial.println(); // EOL
}

void loop() {
  M5.dis.fillpix(0x000000);

  if(banggps.check()) {
    gps_loop();
  }

  if(bangimu.check()) {
    imu_loop();
//    writeSample( outfilename );
  }

  if(banglog.check()) {
    debugSample();
  }

  mselapsed = millis();
}