from mbientlab.metawear import *
from mbientlab.metawear.cbindings import *
import sys


address = sys.argv[1]
device = MetaWear(address)
device.connect()
board = device.board
print("connected to %s" % address)


euler_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(board, SensorFusionData.EULER_ANGLE)
euler_callback = FnVoid_VoidP_DataP(lambda context, data: print("euler %s" % parse_value(data)))
libmetawear.mbl_mw_datasignal_subscribe(euler_signal, None, euler_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(board, SensorFusionData.EULER_ANGLE)
print("activated euler")


accel_signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(board, SensorFusionData.CORRECTED_ACC)
accel_callback = FnVoid_VoidP_DataP(lambda context, data: print("accel %s" % parse_value(data)))
libmetawear.mbl_mw_datasignal_subscribe(accel_signal, None, accel_callback)
libmetawear.mbl_mw_sensor_fusion_enable_data(board, SensorFusionData.CORRECTED_ACC)
print("activated accel")


libmetawear.mbl_mw_sensor_fusion_set_mode(board, SensorFusionMode.NDOF)
libmetawear.mbl_mw_sensor_fusion_write_config(board)
libmetawear.mbl_mw_sensor_fusion_start(board)
print("started")


input("")


libmetawear.mbl_mw_sensor_fusion_stop(board)
libmetawear.mbl_mw_sensor_fusion_clear_enabled_mask(board)
libmetawear.mbl_mw_datasignal_unsubscribe(euler_signal)
libmetawear.mbl_mw_datasignal_unsubscribe(accel_signal)
print("deactivated sensors")


device.disconnect()
sleep(1)
print("disconnected")