from JHUMP_ctrl import *

import six

if __name__ == "__main__":

    # Variables
    cubes = [] 
    
    print("scanning for devices...")
    devices = {}
    def handler(result):
        devices[result.mac] = result.name

    BleScanner.set_handler(handler)
    BleScanner.start()

    sleep(5)
    BleScanner.stop()

    i = 0
    for address, name in six.iteritems(devices):
        print("[%d] %s (%s)" % (i, address, name))
        i+= 1
    
    # B8:27:EB:04:3A:A1
    # 90:48:9A:8C:2F:A8
    
    # Initialize
    #cube = Cube("A", "D8:02:0E:5A:6E:ED", "B8:27:EB:04:3A:A1")
    #cubes.append(cube)

    #cube = Cube("B", "DB:7E:F7:5E:42:D5", "B8:27:EB:04:3A:A1")
    #cubes.append(cube)

    cube = Cube("C", "CC:FA:DC:2D:81:2B", "B8:27:EB:04:3A:A1")
    cubes.append(cube)

    #cube = Cube("D", "DE:1A:EA:BB:05:8D", "B8:27:EB:04:3A:A1")
    #cubes.append(cube)
    
    # Connection
    for c in cubes:
        c.connect()
        #c.reset()
        #print(c.read_charge_status())
        print(c.read_battery_state())

    # Configure Sensors
    base_path = '/home/pi/Apps/JHUMP_project/Sandbox/TestMagn_Result'
    #base_path = '/media/francesco/LENOVO/JHUMP_project/Sandbox/TestMagn_Result/'
    
    ## Accelerometer
    for c in cubes:
        #c.check_acc_type() # Check sensor module type
        c.config_acc(25, 16) # odr=100Hz, range=16g
        c.enable_acc_sampling(base_path) # enable
        c.start_acc_sampling() # start
    
    ## Gyro
    for c in cubes:
        c.config_gyr(GyroBmi160Odr._25Hz, GyroBmi160Range._500dps)
        c.enable_gyr_sampling(base_path) # enable
        c.start_gyr_sampling() # start 
    
    ## Magnetometer
    for c in cubes:
        c.config_magn(MagBmm150Preset.HIGH_ACCURACY) # 20Hz
        c.enable_magn_sampling(base_path)
        c.start_magn_sampling()

    ## Fuser
    #cubes[0].enable_fuser_sampling(base_path)
    #cubes[0].start_fuser_sampling()
        
    print('START')

    sleep(60)

    print('STOP')

    for c in cubes: 
        c.stop_acc_sampling()
        c.stop_gyr_sampling()
        c.stop_magn_sampling()

    for c in cubes:
        c.disable_acc_sampling()
        c.disable_gyr_sampling()
        c.disable_magn_sampling()

        #cubes[0].stop_fuser_sampling()
        #c.disable_fuser_sampling()
        
    sleep(2)

    for c in cubes:
        c.disconnect()

    for c in cubes:
        print("Samples Received from %s_%s" % (c.letter, c.device_mac))
        print("Acc -> %d" % (c.acc_samples))
        print("Gyr -> %d" % (c.gyr_samples))
        print("Magn -> %d" % (c.magn_samples))