#!python
#
# Kudos to https://www.dexterindustries.com
#
# Portions Copyright (c) 2017 Dexter Industries
# Released under the MIT license (http://choosealicense.com/licenses/mit/).
# For more information see https://github.com/DexterInd/DI_Sensors/blob/master/LICENSE.md
#
# File: calibrateIMU

#
# Assists user to perform calibration for NDOF mode
#
# Uses Alan's extended mutex protected SafeIMUSensor() class from ros_safe_inertial_measurement_unit.py
# ros_inertial_measurement_unit.py and rosBNO055.py
#
# Note Does not alter operation mode

from __future__ import print_function
from __future__ import division

import time
from datetime import datetime as dt
from ros_safe_inertial_measurement_unit import SafeIMUSensor
import rosBNO055 as BNO055
from easygopigo3 import EasyGoPiGo3
from math import pi

VERBOSITY = False

def main():
    IMUPORT = "AD1"   # Must be AD1 or AD2 only

    print("\nCalibrate DI IMU (BNO055 chip) without changing mode")
    print("Using mutex-protected, exception-tolerant SW I2C on GoPiGo3 port {}\n".format(IMUPORT))

    imu = SafeIMUSensor(port = IMUPORT, use_mutex = True, init=False, verbose = VERBOSITY)
    time.sleep(1.0)  # allow for all measurements to initialize


    op_mode_str = imu.safe_get_op_mode_str()
    print("Chip operating in {} mode".format(op_mode_str))

    try:
        print("\n ************* Waiting 15s: Please set GoPiGo on floor, in reference direction ******************")
        time.sleep(15)

        print("Resetting BNO055")
        imu.safe_resetBNO055(verbose=VERBOSITY)

        print("\nCurrent Calibration Status:")
        imu.printCalStatus()

        imu.readAndPrint(cr=True)

        # Reseting chip (Heading:0, Roll:0, Pitch:90)
        #imu.safe_resetBNO055(verbose=VERBOSITY)
        # Remap for GoPiGo3: Point-Up, Chip-Toward Front
        #imu.safe_axis_remap(verbose=VERBOSITY)

        if op_mode_str == "NDOF":
            print("\nIf Cal Status not [3,3,0,3], Rotate GoPiGo3 in rocking figure-8 pattern")
            imu.safe_calibrate(verbose=True)
            imu.printCalStatus()
        elif op_mode_str == "IMUPLUS":
            print("\nIMU performs calibration automatically")
            print("No special motion is required")

        print("\nChecking Calibration Status for 1 minute")
        for i in range(0,10):
            imu.printCalStatus(cr=False)
            time.sleep(6)



        print("\n")
        imu.readAndPrint(cr=True)

        print("\nExiting calibrateIMU")

    except KeyboardInterrupt:
        print("\nCntrl-C detected. Exiting..")


# Main loop
if __name__ == '__main__':
	main()

