# usage: python data_fuser.py [mac1] [mac2] ... [mac(n)]
from __future__ import print_function
from ctypes import c_void_p, cast, POINTER
from mbientlab.metawear import MetaWear, libmetawear, parse_value, cbindings
from time import sleep
from threading import Event
from sys import argv

states = []

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

    def data_handler(self, ctx, data):
        values = parse_value(data, n_elem = 3)
        # print(values)
        print("%s -> acc: (%.4f,%.4f,%.4f) \t mag: (%.9f,%.9f,%.9f) \t gyro:(%.5f,%.5f,%.5f)" % (
            self.device.address,
            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))

        self.samples += 1

    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] = mag
        signals[1] = gyro
        print(type(signals))
        print(signals)

        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):

        libmetawear.mbl_mw_mag_bmm150_enable_b_field_sampling(self.device.board)
        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_start(self.device.board)
        libmetawear.mbl_mw_gyro_bmi160_start(self.device.board)
        libmetawear.mbl_mw_acc_start(self.device.board)

        
for i in range(len(argv) - 1):
    d = MetaWear(argv[i + 1])
    d.connect()
    print("Connected to " + d.address)
    states.append(State(d))

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

for s in states:
    s.start()

sleep(10.0)

print("Resetting devices")
events = []
for s in states:
    e = Event()
    events.append(e)

    s.device.on_disconnect = lambda s: e.set()
    libmetawear.mbl_mw_debug_reset(s.device.board)
    print(s.samples)

for e in events:
    e.wait()