#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>  // not used in this demo but required!

// i2c
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();


#include <Arduino.h>
#include <SPI.h>
#include "Adafruit_BLE.h"
#include "Adafruit_BluefruitLE_SPI.h"
#include "Adafruit_BluefruitLE_UART.h"
#include "Adafruit_BLEGatt.h"

#include "BluefruitConfig.h"


// Create the bluefruit object
/* ...hardware SPI, using SCK/MOSI/MISO hardware SPI pins and then user selected CS/IRQ/RST */
Adafruit_BluefruitLE_SPI ble(BLUEFRUIT_SPI_CS, BLUEFRUIT_SPI_IRQ, BLUEFRUIT_SPI_RST);

Adafruit_BLEGatt gatt(ble);

// A small helper
void error(const __FlashStringHelper*err) {
  Serial.println(err);
  while (1);
}

/* The service information */

int32_t htsServiceId;
int32_t htsMeasureCharId;

void setupSensor()  //IMU sensor
{
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G);

  // 2.) Set the magnetometer sensitivity
  lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
  //lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);

  // 3.) Setup the gyroscope
  lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
  //lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}






uint8_t measurement [12] = { 0 }; // Laver et array med 12 pladser - Kaldt measurement til bluetooth del


int B[10]; //x
int A[10]; //y(x)
int runde = 1;
float gennemsnit = 0;
float degrees = 0;
float lastDegrees = 0;



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

  while (!Serial) {
    delay(1); // will pause Zero, Leonardo, etc until serial console opens
  }


  // Try to initialise and warn if we couldn't detect the chip
  if (!lsm.begin())
  {
    Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
    while (1);
  }
  Serial.println("Found LSM9DS1 9DOF");

  // helper to just set the default scaling we want, see above!
  setupSensor();

  /* Initialise the Bluetooth module */
  Serial.print(F("Initialising the Bluefruit LE module: "));

  if ( !ble.begin(VERBOSE_MODE) )
  {
    error(F("Couldn't find Bluefruit, make sure it's in CoMmanD mode & check wiring?"));
  }
  Serial.println( F("OK!") );

  /* Perform a factory reset to make sure everything is in a known state */
  Serial.println(F("Performing a factory reset: "));
  if (! ble.factoryReset() ) {
    error(F("Couldn't factory reset"));
  }

  /* Disable command echo from Bluefruit */
  ble.echo(false);

  Serial.println("Requesting Bluefruit info:");
  /* Print Bluefruit information */
  ble.info();

  // this line is particularly required for Flora, but is a good idea
  // anyways for the super long lines ahead!
  // ble.setInterCharWriteDelay(5); // 5 ms

  Serial.println(F("Adding the Health Thermometer Service definition (UUID = 0x1809): "));
  htsServiceId = gatt.addService(0x1809);
  if (htsServiceId == 0) {
    error(F("Could not add Thermometer service"));
  }

  Serial.println(F("Adding the Temperature Measurement characteristic (UUID = 0x2A1C): "));
  htsMeasureCharId = gatt.addCharacteristic(0x2A1C, GATT_CHARS_PROPERTIES_INDICATE, 12, 12, BLE_DATATYPE_BYTEARRAY);
  if (htsMeasureCharId == 0) {
    error(F("Could not add Temperature characteristic"));
  }

  /* Add the Health Thermometer Service to the advertising data (needed for Nordic apps to detect the service) */
  Serial.print(F("Adding Health Thermometer Service UUID to the advertising payload: "));
  uint8_t advdata[] { 0x02, 0x01, 0x06, 0x05, 0x02, 0x09, 0x18, 0x0a, 0x18 };
  ble.setAdvData( advdata, sizeof(advdata) );

  /* Reset the device for the new service setting changes to take effect */
  Serial.print(F("Performing a SW reset (service changes require a reset): "));
  ble.reset();

  Serial.println();


  KalibrerNul();  //første runde og kalibreringsdata af 0-grader er slut
}
/** Send randomized heart rate data continuously **/


void loop() {

  lsm.read();  /* ask it to read in the data */
  char data = Serial.read();
  Serial.print("runde \t");
  Serial.println(runde);


  if (data = 'k') { // hvis 'k' => Kalibre360
      Kalibrer360();
  }


  /* Get a new sensor event */
  sensors_event_t a, m, g, temp;
  lsm.getEvent(&a, &m, &g, &temp);


  int gyroData_y = lsm.gyroData.y; //skriv y
  degrees = gyroData_y - gennemsnit;

  Serial.print (gyroData_y);
  Serial.print("  -  ");
  Serial.print (gennemsnit);
  Serial.print("  =  ");
  Serial.println(degrees);
  //Serial.println(lsm.gyroData.z);

  degrees += lastDegrees;



  lastDegrees = degrees;
  DataToBluetooth();

  delay(100);
  runde++;

}


void Kalibrer360() {

}


void KalibrerNul() {
    Serial.println("n; \t x[n-1]; \t y= x[n-1]+n \t y[n-1] \t integreret");
    for ( int i = 0 ; i < 10; i++ ) {
      lsm.read();  //lsm (sensoer) læser data
      B[i] = lsm.gyroData.y; //læser w_acc om y-akse
      Serial.println(B[i]); //n
      /*Serial.print("\t ");
        int x = B[i - 1]; //x[n-1]
        Serial.print(x); // + B[runde - 1]+ B[runde - 2]+ B[runde - 3]+ B[runde - 4]+ B[runde - 5]+ B[runde - 6]+ B[runde - 7]+ B[runde - 8]); //data nuvæBende væBdi sammenlægges med davæBende væBdi
        Serial.print("\t\t ");
        int y = B[i] + B[i - 1];
        A[i] = y; // gem y på plads i (i Array A)
        Serial.print(y);//y=x[n-1]+n
        Serial.print("\t\t ");
        Serial.print(A[i - 1]); //y[n-1]
        int z = A[i - 1];
        Serial.print("\t\t ");
        Serial.println(y + z); //Y(n)= x[n]+y[n-1] = integreret
        int sum = B[9] + B[9 - 1]+ A[9 - 1];
        Gns = sum/10;*/
    }
    Serial.print("\t Gns; ");

    float sum = 0; //for at kunne læse sum efter for-loop
    for ( int i = 0 ; i < 10; i++ ) {
      sum += B[i]; //+ B[2] + B[3] + B[4] + B[5] + B[6] + B[7] + B[8] + B[9] + B[10]) / 10);
    }
    gennemsnit = sum / 10;
    Serial.println (gennemsnit);
}

void DataToBluetooth() {
  measurement[1] = ((unsigned int)lsm.accelData.x) / 256;
  measurement[0] = ((unsigned int)lsm.accelData.x) & 255;
  measurement[3] = ((unsigned int)lsm.accelData.y) / 256;
  measurement[2] = ((unsigned int)lsm.accelData.y) & 255;
  measurement[5] = ((unsigned int)lsm.accelData.z) / 256;
  measurement[4] = ((unsigned int)lsm.accelData.z) & 255;

  measurement[7] = ((unsigned int)lsm.gyroData.x) / 256;
  measurement[6] = ((unsigned int)lsm.gyroData.x) & 255;
  //measurement[9] = ((unsigned int)lsm.gyroData.y) / 256;
  //measurement[8] = ((unsigned int)lsm.gyroData.y) & 255;
  measurement[9] = ((unsigned int)degrees) / 256;
  measurement[8] = ((unsigned int)degrees) & 255;


  measurement[11] = ((unsigned int)lsm.gyroData.z) / 256;
  measurement[10] = ((unsigned int)lsm.gyroData.z) & 255; //et array på 12 byte, som overføres til Inventor

  // TODO temperature is not correct due to Bluetooth use IEEE-11073 format
  gatt.setChar(htsMeasureCharId, measurement, 12); //send 12 bytes bliver sat ind, som arg(measurement).. Send 12 elemneter(bytes) i arrayet afsted via bluetooth med krakteristik af htsMeasureCharId
}