#undef DEBUG_GPS

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

#include "config.h"
#include <easywifi.h>
#include <WiFiUdp.h>
#include <OSCMessage.h>
#include <OSCBundle.h>

WiFiUDP Udp;
const IPAddress destination(192, 168, 8, 200);
const unsigned int rxport = 54321;          // listen port
const unsigned int txport = 12345;        // send to port


//char payload[512];
String payload = "";

GPSAnalyse GPS;
Metro banggps(2000);
Metro bangimu(33);  // 30 fps
Metro banglog(1000);

bool sdready = false;
bool recording = false;
String outfn;

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;

String filename_make_sequential() {
  char fn[24];
  for (int n = 1; n < 1000; n++) {
    sprintf(fn, "/wavesample__%.3d.csv", n);
    if ( SD.exists(fn) ) {
      continue;
    } else {
      break;      
    }
  }
  return String(fn);
}

bool sample_write(String filename) {
  File 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 {
    Serial.println("Failed to write sample to SD card");
    return false;
  }
 
  return true;
}

bool sdcard_init() {
  Serial.print("SDcard init...");
  SPI.begin(23, 33, 19, -1);
  if( !SD.begin(-1, SPI) ) {
    Serial.println("SD card mount failed");
    return false;
  } 

  uint8_t cardType = SD.cardType();

  if(cardType == CARD_NONE){
    Serial.println("No SD card attached");
    return false;
  }

  Serial.print("SD Card Type: ");
  
  if(cardType == CARD_MMC){
    Serial.println("MMC");
  } else if(cardType == CARD_SD){
    Serial.println("SDSC");
  } else if(cardType == CARD_SDHC){
    Serial.println("SDHC");
  } else {
    Serial.println("UNKNOWN");
  }

  uint64_t cardSize = SD.cardSize() / (1024 * 1024);
  Serial.printf("SD Card Size: %lluMB\n", cardSize);

  dir_list(SD, "/", 0);
  return true;
}

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

  delay(100);

  // connect to existing wifi access point as client
  if( wifi_connect_as_client(WIFI_SSID, WIFI_PASSWORD) ) {
    // print debugging information
    wifi_print_mode();
    wifi_print_ip(); // print our known ip address
  } else {
    Serial.print("Failed to connect to wifi ");
    Serial.print( WIFI_SSID );
    Serial.println();
  }

  Udp.begin(rxport);
  Serial.print("Listening on port ");
  Serial.println(rxport);
  
  sdready = sdcard_init();
  delay(500);

  outfn = filename_make_sequential();
  
  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 sample_debug() {
    Serial.print( outfn );
    Serial.print(", ");
    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 sample_osc_dispatch() {

  payload = "";
  payload.concat(mselapsed);
  payload.concat(", ");
  payload.concat(gpssample.lat);
  payload.concat(", ");
  payload.concat(gpssample.lon);
  payload.concat(", ");
  payload.concat(imusample.aX);
  payload.concat(", ");
  payload.concat(imusample.aY);
  payload.concat(", ");
  payload.concat(imusample.aZ);
  payload.concat(", ");
  payload.concat(imusample.gX);
  payload.concat(", ");
  payload.concat(imusample.gY);
  payload.concat(", ");
  payload.concat(imusample.gZ);
  payload.concat(", ");
  payload.concat(imusample.pitch);
  payload.concat(", ");
  payload.concat(imusample.roll);
  payload.concat(", ");
  payload.concat(imusample.yaw);
  
//  sprintf(payload, "%d, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s, %s\0",
//    mselapsed, 
//    dtostrf(gpssample.lat),
//    dtostrf(gpssample.lon),
//    dtostrf(imusample.aX),
//    dtostrf(imusample.aY),
//    dtostrf(imusample.aZ),
//    dtostrf(imusample.gX),
//    dtostrf(imusample.gY),
//    dtostrf(imusample.gZ),
//    dtostrf(imusample.pitch),
//    dtostrf(imusample.roll),
//    dtostrf(imusample.yaw),
//    dtostrf(imusample.temp)
//    );

  //Serial.println( payload );

// these two lines craft the message (with an address and a parameter)
  OSCMessage out("/wavesampler/sample");
//  out.add( payload );
  out.add(1.0f*mselapsed);
  out.add(gpssample.lat);
  out.add(gpssample.lon);
  out.add(imusample.aX);
  out.add(imusample.aY);
  out.add(imusample.aZ);
  out.add(imusample.gX);
  out.add(imusample.gY);
  out.add(imusample.gZ);
  out.add(imusample.pitch);
  out.add(imusample.roll);
  out.add(imusample.yaw);

  // these 4 lines that follow send out the message
  Udp.beginPacket(destination, txport);
  out.send(Udp);
  Udp.endPacket();
  out.empty();
}

void osc_route_sample(OSCMessage &msg, int addrOffset ) {
  if( msg.match("/sample/start") ) {
    outfn = filename_make_sequential();
    Serial.print("() Start recording to file ");
    Serial.print(outfn);
    Serial.println("...");
    recording = true;
  } else if( msg.match("/sample/stop") ) { 
    Serial.println("[] STOP recording...");
    recording = false;
  }
}

void osc_message_pump() {
  OSCMessage in;
  int size;

  if( (size = Udp.parsePacket()) > 0)
  {
    // parse incoming OSC message
    while(size--) {
      in.fill( Udp.read() );
    }

    if(!in.hasError()) {
      // if we had never received a message before, make sure we set the flag to avoid triggering 'demo mode'
      //if(!msgReceived) { msgReceived = true; }
//      Serial.print("<< osc message received");
      
      in.route("/sample", osc_route_sample);
    }
  } // if
}

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

  osc_message_pump();

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

  if(bangimu.check()) {
    imu_loop();
    if( recording ) {
      sample_write( outfn );
    }
    //sample_osc_dispatch();
  }

//  if(banglog.check()) {
//    sample_debug();
//  }

  mselapsed = millis();
}