/**************************************************************************
    Calibrate a Sensor
 **************************************************************************/
void calibrate_sensor(BNO080 myIMU) {
    //Enable dynamic calibration for accel, gyro, and mag
    myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag

    //Enable Game Rotation Vector output
    myIMU.enableGameRotationVector(100); //Send data update every 100ms

    //Enable Magnetic Field output
    myIMU.enableMagnetometer(100); //Send data update every 100ms
    myIMU.enableAccelerometer(100);
    myIMU.enableGyro(100);
    
    //Once magnetic field is 2 or 3, run the Save DCD Now command
    Serial.println(F("Calibrating"));
    Serial.println(F("Press 's' to save to flash, or"));
    Serial.println(F("'a' to abort calibration."));
    Serial.println(F("Output in form x, y, z, in uTesla"));

    // The calibration loop
    bool runCal = true;
    while(runCal){
        // First chsck for user input
        if(Serial.available()) {
            byte incoming = Serial.read();
            switch(incoming){
                case 's':
                case 'S':
                    myIMU.saveCalibration(); //Saves the current dynamic calibration data (DCD) to memory
                    myIMU.endCalibration();  //Turns off all calibration
                    Serial.println("Calibration finiahed");
                    runCal = false;
                    break;
                case 'a':
                case 'A':
                    myIMU.endCalibration();  //Turns off all calibration
                    Serial.println("Calibration aborted");
                    runCal = false;
                    break;
                default: {}
            }
        }
        //Look for reports from the IMU
        if (myIMU.dataAvailable() == true && runCal) {
            int magAccuracy = myIMU.getMagAccuracy();
            int accelAccuracy = myIMU.getAccelAccuracy();
            int gyroAccuracy = myIMU.getGyroAccuracy();

            String output = String(magAccuracy) + "," + String(accelAccuracy) + "," + String(gyroAccuracy);

            Serial.println(output);
        }
        delay(500);
    }
}

/**************************************************************************
    Enable a Sensor
 **************************************************************************/
bool enable_sensor(BNO080 myIMU, int address) {
    if (myIMU.begin(address,Wire)) {
        myIMU.enableRotationVector(50); //Send absolute position data update every 50ms
        myIMU.enableAccelerometer (50);  //Send accelerometer data update every 50ms
        return true;
    } else {
        return false;
    }
}