#include <BMI160.h>
#include <CurieIMU.h>
#include <CurieBLE.h>
#include <CurieTime.h>
#include <Adafruit_GPS.h>
#include <SD.h>

#include <string>
#include "defs.h"

#define GPS Serial1

File logfile;
char filename[15];

BLEPeripheral blePeripheral; // create peripheral instance
BLEService ledService("FastApp-2016-04-12");
// create switch characteristic and allow remote device to read and write
BLECharCharacteristic ledCharacteristic("FastApp-2016-04-12", BLERead | BLEWrite);
// create button characteristic and allow remote device to get notifications
BLECharCharacteristic buttonCharacteristic("FastApp-2016-04-12", BLERead | BLENotify); // allows remote device to get notifications

int ax, ay, az;
int gx, gy, gz;

int count = 0; // need a counter because we only get seconds level time from CurieTime.h
String gps="";

void setup() {
  CurieIMU.begin();
  Serial.begin(115200);
  if(GPSECHO) while(!Serial);
  //while(!Serial);
  Serial.println(VERSION);
  // set the local name peripheral advertises
  blePeripheral.setLocalName("FastApp");
  // set the UUID for the service this peripheral advertises:
  blePeripheral.setAdvertisedServiceUuid(ledService.uuid());

  // add service and characteristics
  blePeripheral.addAttribute(ledService);
  blePeripheral.addAttribute(ledCharacteristic);
  blePeripheral.addAttribute(buttonCharacteristic);

  ledCharacteristic.setValue(0);
  buttonCharacteristic.setValue(0);
  //CurieIMU.autoCalibrateGyroOffset();
  //CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  //CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  //CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);

  // Set the accelerometer range to 250 degrees/second
  CurieIMU.setGyroRange(250);

  // advertise the service
  blePeripheral.begin();
  // 9600 NMEA is the default baud rate for Adafruit MTK GPS's- some use 4800
  GPS.begin(9600);

  //GPS.sendCommand(PMTK_SET_NMEA_OUTPUT_RMCONLY);
  //GPS.sendCommand(PMTK_SET_NMEA_UPDATE_5HZ);
  //GPS.sendCommand(PMTK_API_SET_FIX_CTL_5HZ);
  //GPS.sendCommand(PGCMD_NOANTENNA);
  // Ask for firmware version
  //mySerial.println(PMTK_Q_RELEASE);
  setTime(14, 27, 00, 31, 07, 2016);
  // see if the card is present and can be initialized:
  if (!SD.begin(chipSelect)) {      // if you're using an UNO, you can use this line instead
    Serial.println("Card init. failed!");
  }

  strcpy(filename, "GPSLOG00.TXT");
  for (uint8_t i = 0; i < 100; i++) {
    filename[6] = '0' + i/10;
    filename[7] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (!SD.exists(filename)) {
      break;
    }
  }
  logfile = SD.open(filename, FILE_WRITE);
}

void loop() {

  // I'm not sure if this is the best design here, opening and then closing the file everytime the loop repeats... gotta be better way.
  logfile = SD.open(filename, FILE_WRITE);
  boolean endofline=false;

  int i=0;
  while(!endofline){
    if (Serial.available()) {
      char c = Serial.read();
      if(GPSECHO) GPS.write(c);
      gps+=c;
      if(c=='\n') endofline=true;
    }
    if (GPS.available()) {
      char c = GPS.read();
      if(GPSECHO) Serial.write(c);
      gps+=c;
      if(c=='\n') endofline=true;
    }
    i++;
    if(i==1000) break;
  }

  if (endofline) {
    float yaw, pitch, roll;
    int gxRaw, gyRaw, gzRaw;

    CurieIMU.readMotionSensor(ax, ay, az, gx, gy, gz);
    CurieIMU.readGyro(gxRaw, gyRaw, gzRaw);

    // convert the raw gyro data to degrees/second
    yaw = convertRawGyro(gxRaw);
    pitch = convertRawGyro(gyRaw);
    roll = convertRawGyro(gzRaw);

    String gyro = "";

    gyro += "$AGYRO,";
    gyro+= gx; gyro+= ", ";
    gyro+= gy; gyro+= ", ";
    gyro+= gz; gyro+= ", ";
    gyro+= ax; gyro+= ", ";
    gyro+= ay; gyro+= ", ";
    gyro+= az; gyro+= ", ";

    // the yaw, pitch, and roll need typecast to String
    gyro+=String(yaw); gyro+=", ";
    gyro+=String(pitch); gyro+=", ";
    gyro+=String(roll); gyro+=", ";

    //print the timestamp
    gyro += year(); gyro += "-";
    gyro += month(); gyro += "-";
    gyro += day(); gyro += " ";

    gyro += hour(); gyro += ":";
    gyro += minute(); gyro += ":";
    gyro += second();
    if(GPSECHO) Serial.println(gyro);

    // the gps data will contain way more info than we need. we only really need actual location and speed
    logfile.println(gps);

    // the gyro info also spits out way too much data. what is needed here now is the lean angle. the other
    // fields may be able to provide the gravity forces from the seat. hp torque values won't be perfect.
    logfile.println(gyro);

    // clear out the gps string so we can rebuild it.
    gps="";
  }

  logfile.flush();
  logfile.close();
}