// Declination sensor transmitter

#include <CurieBLE.h>
#include <CurieIMU.h>
#include <Wire.h>
#include <DFRobot_QMC5883.h>
#include <math.h>

// UUIDs created using https://www.uuidgenerator.net/
#define IMU_SERVICE_UUID "468c28f3-35a2-4fe8-9e68-be7942c4f152" 
#define IMU_TEMP_UUID "66fbf839-f54e-4162-91a7-8c41464d4b0e"
#define IMU_XYZ_UUID "bd38d6a4-d815-4e50-a9c0-7b9420043151"
#define IMU_SOUTH_UUID "c8ed185c-25d9-4507-a6ad-7aeefcf6cc5e"

// CurieNano acts as a BLE peripheral
BLEPeripheral blePeripheral;

// IMU data is registered as a BLE service
BLEService imuService(IMU_SERVICE_UUID);

// Each IMU data point is its own characteristic
BLECharacteristic imuXYZ(IMU_XYZ_UUID, BLERead | BLENotify,16);
BLECharacteristic imuTemp(IMU_TEMP_UUID, BLERead, 16);
BLECharacteristic imuSouth(IMU_SOUTH_UUID, BLERead, 16);

// Assign pin to indicate BLE connection 
const int INDICATOR_PIN = 13;

DFRobot_QMC5883 compass;

unsigned long microsPerReading, microsPrevious;
float accelScale;
int ax, ay, az, axsum, aysum, azsum;
float mx, my, mxsum, mysum; 
float temperature, southheading;

void setup(){
  
  Serial.begin(9600);

  // Initialize BLE peripheral
  blePeripheral.setLocalName("IMUA");
  blePeripheral.setAdvertisedServiceUuid(imuService.uuid());
  blePeripheral.addAttribute(imuService);
  blePeripheral.addAttribute(imuXYZ);
  blePeripheral.addAttribute(imuTemp);
  blePeripheral.addAttribute(imuSouth);

  // Now, activate the BLE peripheral
  blePeripheral.begin();
  Serial.println("Bluetooth device active, waiting for connections...");
  
  // Initialize HMC5883L
  Serial.println("Initialize HMC5883L");
  while (!compass.begin()){
    Serial.println("Could not find a valid HMC5883L sensor, check wiring!");
    delay(500);
  }

  // Set measurement range
  compass.setRange(HMC5883L_RANGE_1_3GA);

  // Set measurement mode
  compass.setMeasurementMode(HMC5883L_CONTINOUS);

  // Set data rate
  compass.setDataRate(HMC5883L_DATARATE_30HZ);

  // Set number of samples averaged
  compass.setSamples(HMC5883L_SAMPLES_8);

  // start the IMU
  CurieIMU.begin();
  CurieIMU.setAccelerometerRate(25);

  // Set the accelerometer range to 1G
  CurieIMU.setAccelerometerRange(1);

  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();
}

void loop() {
  int n=10;
  int j=0;
  String south = "";
  String southsign = ""; 
  String southdata = "";
  char southbuf[10] = "";
  String temp = "";
  String tempdata = "";  
  char tempbuf[10] = "";

  // Check if the connection to the central is active or not
  BLECentral central = blePeripheral.central();

  if(central) {
    Serial.print("Connected to central: ");
    Serial.println(central.address());
    digitalWrite(INDICATOR_PIN, HIGH);
    
    // Take average of acquired data
    mxsum = 0, mysum = 0, axsum = 0, aysum = 0, azsum = 0;
    for(j=0; j<n; j=j+1){
      updateImuData(); 
      mxsum = mxsum + mx;      
      mysum = mysum + my;
      axsum = axsum + ax;
      aysum = aysum + ay;
      azsum = azsum + az;
      delay(5);
    }
    mxsum = mxsum/n;
    mysum = mysum/n;
    axsum = axsum/n;
    aysum = aysum/n;
    azsum = azsum/n;
    
    SendData();

    // Update IMU south characteristic

    // calculate and update compass south characteristic
    southheading = atan2(mysum,mxsum)*180/PI;
    southdata = String(int(100*abs(southheading)),HEX);
    if (southheading<0){
      southsign = "-";
    } else {
      southsign = "+";
    }
    south = southsign+southdata+"-";
    south.toCharArray(southbuf, 10);
    imuSouth.setValue((unsigned char*)southbuf,10);


    //Update IMU temperature characteristic
    tempdata = String(int(10*temperature),HEX);
    temp = "+"+tempdata+"-";
    temp.toCharArray(tempbuf,10);
    imuTemp.setValue((unsigned char*)tempbuf, 10);

  }else{
    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
    digitalWrite(INDICATOR_PIN, LOW);
  }
}

void updateImuData() {
  int aix, aiy, aiz;
  unsigned long microsNow;
  
  // check if it's time to read data
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {

    // read raw data from CurieIMU
    CurieIMU.readAccelerometer(aix, aiy, aiz);
    ax = aiy;
    ay = aiz;
    az = aix;

    // Read magnetometer
    Vector norm = compass.readNormalize();
    mx = norm.XAxis;
    my = -norm.YAxis;
    
    // Read and calculate IMU sensor temperature
    temperature = CurieIMU.readTemperature();
    temperature = temperature/512 + 23;
    
    // increment previous time to maintain pace
    microsPrevious = microsPrevious + microsPerReading;
  }
}

// Transmit up to three signed four digit Hex numbers
void SendData(){
  char messagebuf[21] = "";
  String message = "";
  String signx = "", signy = "", signz = ""; 
  String xdata = String(int(1*abs(axsum)),HEX);
  String ydata = String(int(1*abs(aysum)),HEX);
  String zdata = String(int(1*abs(azsum)),HEX);
  
  if (axsum<0){
    signx = "-";
  } else {
    signx = "+";
  }
  if (aysum<0){
    signy = "-";
  } else {
    signy = "+";
  }
  if (azsum<0){
    signz = "-";
  } else {
    signz = "+";
  }
  message = signx+xdata+signy+ydata+signz+zdata+"-";
  message.toCharArray(messagebuf, 21);
  imuXYZ.setValue((unsigned char*)messagebuf,21);
  Serial.print(messagebuf); 
  Serial.print("\n");
}