from JHUMP_ctrl import *

if __name__ == "__main__":

    # Variables
    cubes = [] 
        
    # B8:27:EB:04:3A:A1
    # 90:48:9A:8C:2F:A8
    
    print("scanning for devices...")
    devices = {}
    def handler(result):
        devices[result.mac] = result.name

    BleScanner.set_handler(handler)
    BleScanner.start()

    sleep(10)
    BleScanner.stop()
    
    # 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)
    
    #cube = Cube("E", "CD:CB:1E:37:11:03", "B8:27:EB:04:3A:A1")
    #cubes.append(cube)
    
    #cube = Cube("F", "E2:01:5B:FC:05:C1", "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())

    sleep(1)

    # 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(1)

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

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