from __future__ import print_function
import time
from mbientlab.metawear.cbindings import *
from mbientlab.metawear import MetaWear, libmetawear, parse_value
from time import sleep
from threading import Event


class State:
    def __init__(self, device):
        self.device = device
        self.samples_received = 0
        self.callback = FnVoid_VoidP_DataP(self.data_handler)
        self.data_list = []
        self.processor = None

    def data_handler(self, ctx, data):
        print("%s -> %s -> %s - %s  %s " % (self.device.address,
                                            parse_value(data),
                                            data.contents.epoch,
                                            int(time.time()*1000),
                                            cast(data.contents.extra, POINTER(c_uint)).contents.value))
        self.data_list.append([data.contents.epoch, time.time()*1000])
        self.samples_received += 1

    def setup(self):
        libmetawear.mbl_mw_settings_set_connection_parameters(self.device.board, 50, 50, 0, 6000)
        sleep(1.5)

        e = Event()

        def processor_created(context, pointer):
            self.processor = pointer
            e.set()

        fn_wrapper = FnVoid_VoidP_VoidP(processor_created)

        # setup acc
        #libmetawear.mbl_mw_acc_set_odr(self.device.board, 100.0)
        #libmetawear.mbl_mw_acc_set_range(self.device.board, 16.0)
        #libmetawear.mbl_mw_acc_write_acceleration_config(self.device.board)
        #signal = libmetawear.mbl_mw_acc_get_acceleration_data_signal(self.device.board)

        #setup quats
        libmetawear.mbl_mw_sensor_fusion_set_mode(self.device.board, SensorFusionMode.NDOF)
        libmetawear.mbl_mw_sensor_fusion_set_acc_range(self.device.board, SensorFusionAccRange._8G)
        libmetawear.mbl_mw_sensor_fusion_set_gyro_range(self.device.board, SensorFusionGyroRange._2000DPS)
        libmetawear.mbl_mw_sensor_fusion_write_config(self.device.board)
        signal = libmetawear.mbl_mw_sensor_fusion_get_data_signal(self.device.board, SensorFusionData.QUATERNION)

        print('create Accounter')
        # count
        # libmetawear.mbl_mw_dataprocessor_accounter_create_count(signal, None, fn_wrapper)
        # timestamp mode
        libmetawear.mbl_mw_dataprocessor_accounter_create(signal, None, fn_wrapper)
        e.wait()
        print('subscribe to datasignal')
        libmetawear.mbl_mw_datasignal_subscribe(self.processor, None, self.callback)

    def start(self):
        print('Start Data')
        # start quats
        libmetawear.mbl_mw_sensor_fusion_enable_data(self.device.board, SensorFusionData.QUATERNION)
        libmetawear.mbl_mw_sensor_fusion_start(self.device.board)
        # start acc
        #libmetawear.mbl_mw_acc_enable_acceleration_sampling(self.device.board)
        #libmetawear.mbl_mw_acc_start(self.device.board)

    def reset(self):

        libmetawear.mbl_mw_led_stop_and_clear(self.device.board)
        libmetawear.mbl_mw_logging_stop(self.device.board)
        libmetawear.mbl_mw_acc_stop(self.device.board)
        libmetawear.mbl_mw_logging_clear_entries(self.device.board)
        libmetawear.mbl_mw_macro_erase_all(self.device.board)

        libmetawear.mbl_mw_debug_reset_after_gc(self.device.board)

        event = Event()
        self.device.on_disconnect = lambda status: event.set()
        libmetawear.mbl_mw_debug_disconnect(self.device.board)
        event.wait()


def main():
    states = []

    list_of_devices = ['CE:70:B9:XX:XX:XX']

    for device in list_of_devices:
        d = MetaWear(device)
        d.connect()
        print("Connected to " + d.address)
        states.append(State(d))

    for state in states:
        state.setup()

    for state in states:
        state.start()

    sleep(10)
    print('reset sensors')
    for state in states:
        state.reset()


if __name__ == '__main__':
    main()