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

// i2c
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
// You can also use software SPI
//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_SCK, LSM9DS1_MISO, LSM9DS1_MOSI, LSM9DS1_XGCS, LSM9DS1_MCS);
// Or hardware SPI! In this case, only CS pins are passed in
//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_XGCS, LSM9DS1_MCS);



unsigned long now = 0, last = 0; //micros() timers for AHRS loop
float deltat = 0;  //loop time in seconds

#define SPEED 50 // ms between angle prints
unsigned long lastPrint = 0; // Keep track of print time
// Vector to hold quaternion
static float q[4] = {1.0, 0.0, 0.0, 0.0};
// We save the orientation here:
// 0 - yaw
// 1 - pitch
// 2 - roll
static float angles[3];

float pitch_cal = 0;
float yaw_cal = 0;
float roll_cal = 0;


void setupSensor()
{
  // 1.) Set the accelerometer range
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ);
  //lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ);
  
  // 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);
}

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

  while (!Serial) {
    delay(1);
  }
  
  Serial.println("LSM9DS1 data read demo");
  
  // 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");

  setupSensor();

  static float Gxyz[3], Axyz[3], Mxyz[3];
  delay(100);
  Serial.println("CALIBRATING HOLD STILL");
  delay(500);
  for (int i=0; i < 50; i++) {
    lsm.read();  /* ask it to read in the data */ 

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

    get_scaled_IMU(Gxyz, Axyz, Mxyz, g.gyro.x, g.gyro.y, g.gyro.z, a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z);
    now = micros();
    deltat = (now - last) * 1.0e-6; //seconds since last update
    last = now;
    mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q);
    get_angles(angles, q);
    delay(SPEED);
    yaw_cal += (angles[0]);
    pitch_cal += abs(angles[1]);
    roll_cal += abs(angles[2]);
  }
  last = 0;
  yaw_cal /= 50;
  pitch_cal /= 50;
  roll_cal /= 50;
  Serial.println("CALIBRATION DONE!");
  Serial.println(pitch_cal);
  Serial.println(roll_cal);

}

void loop() 
{
   static int loop_counter = 0;
   static float Gxyz[3], Axyz[3], Mxyz[3]; // Gyro/Acc/Mag data
   lsm.read();  /* ask it to read in the data */ 

   /* Get a new sensor event */ 
   sensors_event_t a, m, g, temp;
   lsm.getEvent(&a, &m, &g, &temp); 
   loop_counter++;
   get_scaled_IMU(Gxyz, Axyz, Mxyz, g.gyro.x, g.gyro.y, g.gyro.z, a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z);
   now = micros();
   deltat = (now - last) * 1.0e-6; //seconds since last update
   last = now;
   mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q);
   get_angles(angles, q);
   delay(SPEED);
   if (loop_counter == 30) {
    Serial.print("Yaw: ");
    Serial.println(angles[0]-yaw_cal);
    Serial.print("Pitch: ");
    Serial.println(angles[1]-pitch_cal);
    Serial.print("Roll: ");
    Serial.println(angles[2]+roll_cal);
    loop_counter = 0;
   }
   
}