#include <SD.h>
#include <SPI.h>
#include <WiFi101.h>
#include <WiFiUdp.h>
#include <EEPROM.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_RGBLCDShield.h>
#include <utility/Adafruit_MCP23017.h>
#include "SparkFun_BNO080_Arduino_Library.h"

#include "arduino_secrets.h" 

#define ON 0x1
#define OFF 0x0

int SAMPLE_DELAY = 50;   // In Milliseconds
int LOG_PERIOD = 30;     // In Seconds
int NUMOFSAMPLES = (1000/SAMPLE_DELAY)*LOG_PERIOD; // This count is approximate
int backlightTO = NUMOFSAMPLES;

// The current states of the LCD shield buttons
uint8_t buttons;
bool upBtn  = false;
bool dwnBtn = false;
bool lftBtn = false;
bool rgtBtn = false;
bool selBtn = false;

String accuracy1;    // Sensor 1 Live accuracy in degrees
String accuracy2;    // Sensor 2 Live accuracy in degrees
String shldStatus;   // Indicator that WIFI shield initialization was accompished
String udpStatus;    // Indicator that UDP initialization was accompished
String sdStatus;     // SD interface state

String connSSID;     // The WIFI SSID that the system is connected to
String connIPaddr;   // The system's WIFI address
String connSS;       // The signal strength of the connection when it was established

String timeStringStart;  // The system start time
String dateStringStart;  // The system start date
String timeString;       // The last time fetched
String dateString;       // The last date fetched

WiFiUDP Udp;         // A UDP instance to let us send/receive packets over UDP
                     // Note that there can only be one of these...
                     
Adafruit_RGBLCDShield lcd = Adafruit_RGBLCDShield();

sensors_event_t event;
BNO080 IMU1;              // pointer for sensor 1
bool IMU1Present = true;    // sensor 1 enable
BNO080 IMU2;              // pointer for sensor 2
bool IMU2Present = true;    // sensor 2 enable

File myFile;            // pointer to open logging file
bool FileOK = false;    // Is the SD file system good?
bool firstLoop = true;  // true for first iteration of the loop
bool showData = false;  // print output to the serial port
bool showAccuracy = false;  // print output to the serial port
float quatAccuracy;

/**************************************************************************
    Arduino setup function (automatically called at startup)
 **************************************************************************/
void setup(void) {
   
    Serial.begin(9600);
    lcd.begin(16, 2);
    lcd.setBacklight(ON);
    Serial.println(F("Serial Port Initialized"));
    lcd.print("LCD Initialized");
    
    initBlinkingLED();
    
    Serial.println(F("Initializing WIFI Shield"));
    initWIFIShield();

    Serial.println(F("Initializing WIFI Connection"));
    connectWIFI();
    printWiFiStatus();

    Serial.println(F("Initializing UDP"));
    initTimePort();     // open the local port for NTP
    initNTPRequest();   // initialize the NTP request packet
    udpStatus = "UDP Running";

    Serial.println(F("Initializing SD Card Interface"));
    initSDCard();

    Wire.begin();
    Wire.setClock(400000); //Increase I2C data rate to 400kHz

    /************ Start of Sensor 1 code ************/
    Serial.print(F("\nInitializing sensor 1... "));
    if(IMU1Present) {
        if (!IMU1.begin(0x4B,Wire)) {
            Serial.println("failed");
            IMU1Present = false;
        } else {
            IMU1.enableRotationVector(50); //Send absolute position data update every 50ms
            IMU1.enableAccelerometer(50);  //Send accelerometer data update every 50ms
            Serial.println("good");
            Serial.println(F("Rotation vector 1 enabled"));
        }
    } else {
      Serial.println(F("not present"));
    }
    /************* End of Sensor 1 code *************/
    
    /************ Start of Sensor 2 code ************/
    Serial.print(F("\nInitializing sensor 2... "));
    if(IMU2Present) {
        if (!IMU2.begin(0x4A,Wire)) {
            Serial.println("failed");
            IMU2Present = false;
        } else {
            IMU2.enableRotationVector(50); //Send absolute positiondata update every 50ms
            IMU2.enableAccelerometer(50);  //Send accelerometer data update every 50ms
            Serial.println("good");
            Serial.println(F("Rotation vector 2 enabled"));
        }
    } else {
        Serial.println(F("not present"));
    }
    /************* End of Sensor 2 code *************/

}

/**************************************************************************
    Loop()
 **************************************************************************/
void loop() {

    String output;
    String output1;
    String output2;
    
    // do all the initialization that can only happen on the first
    // iteration of the main loop...
    if(firstLoop){
        sendNTPpacket(); 
        delay(1000);
        String dateTimeString = receiveNTPpacket();
        dateStringStart = dateString;
        timeStringStart = timeString;
        Serial.println();
        Serial.print("System up time: ");
        Serial.println(dateTimeString);
        Serial.println();
        modeDescriptors();
    }
    
    readNavButtons();   //  Get Button values for use through out the loop
     
    /* Get Sensor 1 data */
    if(IMU1Present){
        if (IMU1.dataAvailable() == true) {
            float quatI = IMU1.getQuatI();
            float quatJ = IMU1.getQuatJ();
            float quatK = IMU1.getQuatK();
            float quatReal = IMU1.getQuatReal();
        
            float x = IMU1.getAccelX();
            float y = IMU1.getAccelY();
            float z = IMU1.getAccelZ();
        
            accuracy1 = IMU1.getQuatRadianAccuracy()*57.2958;
            output1 = "0," + String(quatReal,4) + "," + String(quatI,4) + "," + String(quatJ,4) + "," + String(quatK,4) + "," + String(x,2) + "," + String(y,2) + "," + String(z,2);
        } else {
            output1 =  ",,,,,,";
            accuracy1 = "";
        }
    }

    /* Get Sensor 2 data */
    if(IMU2Present){
        if (IMU2.dataAvailable() == true) {
            float quatI = IMU2.getQuatI();
            float quatJ = IMU2.getQuatJ();
            float quatK = IMU2.getQuatK();
            float quatReal = IMU2.getQuatReal();
        
            float x = IMU2.getAccelX();
            float y = IMU2.getAccelY();
            float z = IMU2.getAccelZ();
        
            accuracy2 = IMU2.getQuatRadianAccuracy()*57.2958;
            output2 = "1," + String(quatReal,4) + "," + String(quatI,4) + "," + String(quatJ,4) + "," + String(quatK,4) + "," + String(x,2) + "," + String(y,2) + "," + String(z,2);
        } else {
            output2 =  ",,,,,,";
            accuracy2 = "";
        }
    }

    output = output1 + "," + output2;

    sendOutputPacket(output1);  // transmit output to LabVIEW via UDP
    sendOutputPacket(output2);  // transmit output to LabVIEW via UDP

    if (showAccuracy) {
        Serial.print(accuracy1);
        Serial.print("\t");
        Serial.println(accuracy2);
    }

    if (showData) {
        Serial.println(output);
    }
    
    checkModes();   // check for serial inputs

    blinkTheLED();  // alternate the blink LED between on and off states
    
    checkLogStart();    // check to see if logging has been started
    logData(output);    // If turned on, do logging
   
    selectStatusDisplay();  // Select row and column to display
    selectStatusStrings();  // Turn rows and column into strings
    showStatus();           // Display the selected Strings

    // if count reaches 0, turnoff backlight
    if (backlightTO<=0){
        lcd.setBacklight(OFF);
    }
    
    /* Wait the specified delay before requesting new data */
    firstLoop = false;
    delay(SAMPLE_DELAY);
}