/*
 * This program feeds simulated data to the LabVIEW-based
 * Interface Engine over a UDP feed.
 */

// Include the networking stuff
#include <SPI.h>
#include <WiFi101.h>
#include <WiFiUdp.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>

int status = WL_IDLE_STATUS;
#include "arduino_secrets.h" 
///////please enter your sensitive data in the Secret tab/arduino_secrets.h
char ssid[] = SECRET_SSID;    // your network SSID (name)
char pass[] = SECRET_PASS;    // your network password (use for WPA, or use as key for WEP)
int keyIndex = 0;             // your network key Index number (needed only for WEP)

IPAddress remote_ip(10, 0, 0, 102);  // The PC running LabVIEW
unsigned int remote_port = 61558;    // The UDP port that LabVIEW is watching
unsigned int localPort = 8888;       // The UDP port that the Arduino is monitoring
char packetBuffer[145];              // character array to hold outgoing message

WiFiUDP Udp;   // A UDP instance to let us send and receive packets over UDP
Adafruit_BNO055 bno = Adafruit_BNO055();   // Sensor Instance

void setup() {

  Serial.begin(9600);
  
  // check for the presence of the shield:
  if (WiFi.status() == WL_NO_SHIELD) {
    Serial.println("WiFi shield not present");
    // don't continue:
    while (true);
  }

  // attempt to connect to WiFi network:
  while ( status != WL_CONNECTED) {
    Serial.print("Attempting to connect to SSID: ");
    Serial.println(ssid);
    // Connect to WPA/WPA2 network. Change this line if using open or WEP network:
    status = WiFi.begin(ssid, pass);
    // wait 10 seconds for connection:
    delay(2000);
  }

  Serial.println("Connected to wifi");
  printWiFiStatus();

  Serial.println("\nStarting UDP Instance...");
  Udp.begin(localPort);

    /* Initialise the sensor */
  if(!bno.begin())
  {
    /* There was a problem detecting the BNO055 ... check your connections */
    Serial.print("No BNO055 detected");
    while(1);
  }
  delay(1000);
  bno.setExtCrystalUse(true);
}

void loop() {

    // Generate Channel 1 Simulated Data
    // to be replaced with actual acquisition logic
    float hq0 = ((float)random(200, 999))/1000;
    float hq1 = ((float)random(200, 999))/1000;
    float hq2 = ((float)random(200, 999))/1000;
    float hq3 = ((float)random(200, 999))/1000;

    // Generate output packet
    imu::Quaternion quat = bno.getQuat();
    String cn0 = String(quat.w(),4) + "," + String(quat.x(),4) + "," + String(quat.y(),4) + "," + String(quat.z(),4);
    
    String cn1 = String(hq0,4) + "," + String(hq1,4) + "," + String(hq2,4) + "," + String(hq3,4);
    String output = cn0 + "," + cn1;

    // Print output to serial monitor
    // Serial.println(output);

    // Copy string into outgoing packet --
    // Stop when you get to the null terminator
    int i = 0;
    memset(packetBuffer, 0, 144);
    while ((int)output[i] != 0){
      packetBuffer[i] = output[i];
      i = i + 1;
    }
  
    // send the output via UDP
    Udp.beginPacket(remote_ip, remote_port);
    Udp.write(packetBuffer);
    Udp.endPacket();
    
    delay(250);    
}

void printWiFiStatus() {
  // print the SSID of the network you're attached to:
  Serial.print("SSID: ");
  Serial.println(WiFi.SSID());

  // print your WiFi shield's IP address:
  IPAddress ip = WiFi.localIP();
  Serial.print("IP Address: ");
  Serial.println(ip);

  // print the received signal strength:
  long rssi = WiFi.RSSI();
  Serial.print("signal strength (RSSI):");
  Serial.print(rssi);
  Serial.println(" dBm");
}