# usage: python log_acc.py [mac]
from __future__ import print_function
from mbientlab.metawear import MetaWear, libmetawear, parse_value, create_voidp, create_voidp_int
from mbientlab.metawear.cbindings import *
from time import sleep
from threading import Event

import sys


d = MetaWear('x')
d.connect()
print("Connected to " + d.address)

print("Configuring device")


try:

    # Euler angles
    libmetawear.mbl_mw_sensor_fusion_set_mode(d.board, SensorFusionMode.IMU_PLUS)

    libmetawear.mbl_mw_sensor_fusion_enable_data(d.board, SensorFusionData.EULER_ANGLE)
    libmetawear.mbl_mw_sensor_fusion_enable_data(d.board, SensorFusionData.CORRECTED_ACC)
    libmetawear.mbl_mw_sensor_fusion_enable_data(d.board, SensorFusionData.CORRECTED_GYRO)

    libmetawear.mbl_mw_sensor_fusion_write_config(d.board)

    signal_acc = libmetawear.mbl_mw_sensor_fusion_get_data_signal(d.board, SensorFusionData.CORRECTED_ACC)
    logger_acc = create_voidp(lambda fn: libmetawear.mbl_mw_datasignal_log(signal_acc, None, fn), resource="acc_logger")

    signal_gyro = libmetawear.mbl_mw_sensor_fusion_get_data_signal(d.board, SensorFusionData.CORRECTED_GYRO)
    logger_gyro = create_voidp(lambda fn: libmetawear.mbl_mw_datasignal_log(signal_gyro, None, fn), resource="gyro_logger")

    signal_euler = libmetawear.mbl_mw_sensor_fusion_get_data_signal(d.board, SensorFusionData.EULER_ANGLE)
    logger_euler = create_voidp(lambda fn: libmetawear.mbl_mw_datasignal_log(signal_euler, None, fn), resource="euler_logger")

    # Start logging
    libmetawear.mbl_mw_logging_start(d.board, 0)

    libmetawear.mbl_mw_sensor_fusion_start(d.board)

    print("Logging data for 15s")
    sleep(2)

    libmetawear.mbl_mw_sensor_fusion_stop(d.board)

    # Stop logging
    libmetawear.mbl_mw_logging_stop(d.board)


    print("Downloading data")
    libmetawear.mbl_mw_settings_set_connection_parameters(d.board, 7.5, 7.5, 0, 6000)
    sleep(1.0)

    e = Event()

    def progress_update_handler(context, entries_left, total_entries):
        if (entries_left == 0):
            e.set()

    fn_wrapper = FnVoid_VoidP_UInt_UInt(progress_update_handler)
    download_handler = LogDownloadHandler(context=None,
                                          received_progress_update=fn_wrapper,
                                          received_unknown_entry=cast(None, FnVoid_VoidP_UByte_Long_UByteP_UByte),
                                          received_unhandled_entry=cast(None, FnVoid_VoidP_DataP))

    callback_acc = FnVoid_VoidP_DataP(lambda ctx, p: print("Acc: {epoch: %d, value: %s}" % (p.contents.epoch, parse_value(p))))
    callback_gyro = FnVoid_VoidP_DataP(lambda ctx, p: print("Gyro: {epoch: %d, value: %s}" % (p.contents.epoch, parse_value(p))))
    callback_euler = FnVoid_VoidP_DataP(lambda ctx, p: print("Euler: {epoch: %d, value: %s}" % (p.contents.epoch, parse_value(p))))

    libmetawear.mbl_mw_logger_subscribe(callback_euler, None, callback_euler)
    libmetawear.mbl_mw_logger_subscribe(logger_acc, None, callback_acc)
    libmetawear.mbl_mw_logger_subscribe(logger_gyro, None, callback_gyro)

    libmetawear.mbl_mw_logging_download(d.board, 0, byref(download_handler))
    e.wait()
    libmetawear.mbl_mw_logger_remove(logger_acc)
    libmetawear.mbl_mw_logger_remove(callback_euler)
    libmetawear.mbl_mw_logger_remove(logger_gyro)



except RuntimeError as err:
    print(err)
finally:
    print("Resetting device")

    e = Event()
    d.on_disconnect = lambda status: e.set()
    libmetawear.mbl_mw_debug_reset(d.board)
    e.wait()