from __future__ import print_function
from ctypes import c_void_p, cast, POINTER
from mbientlab.metawear import MetaWear, libmetawear, parse_value, cbindings, MagBmm150Preset
from threading import Event
from time import sleep
states = []


class State:
    def __init__(self, device):
        self.device = device
        self.callback = cbindings.FnVoid_VoidP_DataP(self.data_handler)
        self.processor = None

    def data_handler(self, ctx, data):
        values = parse_value(data, n_elem=3)
        print("acc: (%.4f,%.4f,%.4f), gyro; (%.4f,%.4f,%.4f), mag; (%.6f,%.6f,%.6f)" % (
        values[0].x, values[0].y, values[0].z, values[1].x, values[1].y, values[1].z, values[2].x, values[2].y, values[2].z))

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

        e = Event()

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

        fn_wrapper = cbindings.FnVoid_VoidP_VoidP(processor_created)

        acc = libmetawear.mbl_mw_acc_get_acceleration_data_signal(self.device.board)
        gyro = libmetawear.mbl_mw_gyro_bmi160_get_rotation_data_signal(self.device.board)
        mag = libmetawear.mbl_mw_mag_bmm150_get_b_field_data_signal(self.device.board)
        signals = (c_void_p * 2)()
        signals[0] = gyro
        signals[1] = mag
        libmetawear.mbl_mw_dataprocessor_fuser_create(acc, signals, 2, None, fn_wrapper)
        e.wait()

        libmetawear.mbl_mw_datasignal_subscribe(self.processor, None, self.callback)

    def start(self):
        '''
                _10Hz = 0
                _2Hz = 1
                _6Hz = 2
                _8Hz = 3
                _15Hz = 4
                _20Hz = 5
                _25Hz = 6
                _30Hz = 7
        '''
        libmetawear.mbl_mw_mag_bmm150_set_preset(self.device.board, MagBmm150Preset.HIGH_ACCURACY)
        libmetawear.mbl_mw_mag_bmm150_configure(self.device.board, 9, 15, 5)
        libmetawear.mbl_mw_gyro_bmi160_enable_rotation_sampling(self.device.board)
        libmetawear.mbl_mw_acc_enable_acceleration_sampling(self.device.board)
        libmetawear.mbl_mw_mag_bmm150_enable_b_field_sampling(self.device.board)

        libmetawear.mbl_mw_gyro_bmi160_start(self.device.board)
        libmetawear.mbl_mw_acc_start(self.device.board)
        libmetawear.mbl_mw_mag_bmm150_start(self.device.board)

def start_data_stream(device):
    states.append(State(device))

    for s in states:
        print("Configuring %s" % (s.device.address))
        s.setup()

    for s in states:
        s.start()

    sleep(10.0)

    return states