#!python
#
#
# File: startIMU

# Purpose: Start BNO055 in NDOF (default) or IMUPLUS mode
#

"""
# Usage: startIMU [-h] [-i] [-v] [-p {AD1,AD2}]

optional arguments:
  -h, --help            show this help message and exit
  -i, --imuplus         start in imuplus mode (no mags)
  -v, --verbose         detailed output
  -p {AD1,AD2}, --port {AD1,AD2}
                        port 'AD1' (default) or 'AD2' only

"""
#
#
# Uses Alan's extended mutex protected SafeIMUSensor() class from ros_safe_inertial_measurement_unit
# (which uses ros_inertial_measurement_unit which uses rosBNO055)
# that allows passing/setting fusion mode:
#    BNO055.OPERATION_MODE_NDOF    (Fusion using gyros, accellerometers and magnemometers)
# or BNO055.OPERATION_MODE_IMUPLUS (Fusion using gyros, accellerometers only. No magnemometers)
# to start the BNO055, and set heading, roll, pitch to 0,0,0)
#

from __future__ import print_function
from __future__ import division

import argparse

import time
from datetime import datetime as dt
from ros_safe_inertial_measurement_unit import SafeIMUSensor
import rosBNO055 as BNO055



def main():

    # ARGUMENT PARSER
    ap = argparse.ArgumentParser()
    ap.add_argument("-i", "--imuplus",  default=False, action='store_true',
                     help="start in imuplus mode (no mags)")
    ap.add_argument("-v", "--verbose",  default=False, action='store_true',
                     help="detailed output")
    ap.add_argument("-p", "--port", default="AD1", choices=['AD1', 'AD2'], help="port 'AD1' (default) or 'AD2' only")

    args = vars(ap.parse_args())
    mode_imuplus = args['imuplus']
    VERBOSITY = args['verbose']
    IMUPORT = args['port']

    print("\nStart DI IMU (BNO055 chip)")


    if (mode_imuplus == True):
        REQ_MODE = BNO055.OPERATION_MODE_IMUPLUS
        print("Using mutex-protected, exception-tolerant SW I2C")
        print("on GoPiGo3 port {} in IMUPLUS mode\n".format(IMUPORT))
    else:
        REQ_MODE = BNO055.OPERATION_MODE_NDOF
        print("Using mutex-protected, exception-tolerant SW I2C")
        print("on GoPiGo3 port {} in NDOF mode\n".format(IMUPORT))


    try:
        imu = SafeIMUSensor(port = IMUPORT, use_mutex = True, mode = REQ_MODE, init=True, verbose = VERBOSITY)
    except Exception as e:
        print("SafeIMUSensor object instantiation error - check IMU is attached to port {}".format(str(IMUPORT)))
        exit(1)

    time.sleep(1.0)  # allow for all measurements to initialize

    exCnt = imu.getExceptionCount()

    op_mode_str = imu.safe_get_op_mode_str()

    if (VERBOSITY == True): 
        print("Initialized in {} mode".format(op_mode_str))

        if (op_mode_str=="IMUPLU"):
            print("(Fusion based on Gyros, and Accels - no mags)")
        elif (op_mode_str=="NDOF"):
            print("(Fusion based on Gyros, Accels, and Mags)")

        imu.printCalStatus()
        print("\n")

    imu.safe_resetBNO055(verbose=VERBOSITY)


    try:
        print("IMU READING AFTER RESET:")
        imu.readAndPrint(cr=True)
        print("\nExiting startIMU")

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


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

