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 time
import sys


imu_data = []
        
def disconnect_download():
    sleep(3)             
    if d.is_connected:   
        d.disconnect()  

def callback(ctx, data):
        parsed = parse_value(data)
        values = [(data.contents.epoch), parsed.x, parsed.y, parsed.z,]
        imu_data.append(values)

def connect_device(d):
    while not d.is_connected:
        print("Connecting...")
        try:
            d.connect()
            if d.is_connected:
                print("connected")
        except:
            print("Could not connect, retrying...")
            sleep(5)
try:
    print("Searching for device...")
    d = MetaWear(sys.argv[1])
    connect_device(d)
    print("Connected to " + d.address)

    print("Configuring device")
    libmetawear.mbl_mw_logging_clear_entries(d.board)
    libmetawear.mbl_mw_metawearboard_tear_down(d.board)
    
    signal = libmetawear.mbl_mw_acc_get_acceleration_data_signal(d.board)
    logger = create_voidp(lambda fn: libmetawear.mbl_mw_datasignal_log(signal, None, fn), resource = "acc_logger")
    libmetawear.mbl_mw_logging_start(d.board, 0)
    libmetawear.mbl_mw_acc_enable_acceleration_sampling(d.board)
    libmetawear.mbl_mw_acc_bmi160_set_odr(d.board, AccBmi160Odr._50Hz)
    libmetawear.mbl_mw_acc_write_acceleration_config(d.board)
    libmetawear.mbl_mw_acc_start(d.board)
    libmetawear.mbl_mw_settings_set_connection_parameters(d.board, 7.5, 7.5, 0, 6000)
    
    for i in range(10): 
        
        print("Logging data for 25s")
        sleep(25)
        
        connect_device(d)

        print("Downloading data...")

        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 = FnVoid_VoidP_DataP(callback)
        libmetawear.mbl_mw_logger_subscribe(logger, None, callback)
        libmetawear.mbl_mw_logging_download(d.board, 0, byref(download_handler))
        
        d.on_disconnect = lambda status: e.set()
        
        #FORCE DISCONNECT DURING DOWNLOAD
        disconnect_download()    #comment out to test walking out of range disconnection
       
        e.wait()
        d.on_disconnect = lambda status: print("dc") 
                
        print("\nData Length: ", (imu_data[-1][0] - imu_data[0][0])/1000, "seconds")
        print("Start Time: ", time.ctime(imu_data[0][0]/1000))
        print("End Time: ", time.ctime(imu_data[-1][0]/1000), "\n")
        imu_data = [] 
    
    connect_device(d) 
    libmetawear.mbl_mw_acc_stop(d.board)
    libmetawear.mbl_mw_acc_disable_acceleration_sampling(d.board)
    libmetawear.mbl_mw_logging_stop(d.board)
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()