import board
import time
import digitalio
import busio
from adafruit_debouncer import Button
from adafruit_ble import BLERadio
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement
from adafruit_ble.services.nordic import UARTService
from adafruit_bluefruit_connect.raw_text_packet import RawTextPacket
import adafruit_lis3dh

# Set up I2C for accelerometer
i2c = busio.I2C(board.ACCELEROMETER_SCL, board.ACCELEROMETER_SDA)
int1 = digitalio.DigitalInOut(board.ACCELEROMETER_INTERRUPT)
accelerometer = adafruit_lis3dh.LIS3DH_I2C(i2c, address=0x19, int1=int1)
accelerometer.range = adafruit_lis3dh.RANGE_8_G

# Name of advertised device
receiver_name = "ACF-r"

ble = BLERadio()
uart_connection = None

def send_packet(uart_connection_name, packet):
    """Returns False if no longer connected."""
    try:
        uart_connection_name[UARTService].write(packet.to_bytes())
    except:  # pylint: disable=bare-except
        try:
            uart_connection[UARTService].write(packet)
        except:  # pylint: disable=bare-except
            try:
                uart_connection_name.disconnect()
            except:  # pylint: disable=bare-except
                pass
            print("No longer connected")
            return False
    return True


def send_restart(uart_connection_name):
    restart_packet = RawTextPacket("RESTART".encode('utf-8'))
    if not send_packet(uart_connection_name, restart_packet):
        print("Failed to send restart message. Disconnecting...")
        uart_connection_name.disconnect()




while True:
    if not uart_connection or not uart_connection.connected:
        print("Scanning...")
        for adv in ble.start_scan(ProvideServicesAdvertisement, timeout=5):
            if UARTService in adv.services:
                if adv.complete_name == receiver_name:
                    uart_connection = ble.connect(adv)  # Create a UART connection...
                    print(f"I've found and connected to {receiver_name}!")
                    break  # MUST include this here or code will never continue after connection.
                # Stop scanning whether or not we are connected.
                ble.stop_scan()  # And stop scanning.

    while uart_connection and uart_connection.connected:
        # Read accelerometer values
        x, y, z = accelerometer.acceleration
        print(f"Accelerometer: x={x}, y={y}, z={z}")

        data_to_send = str(x) + "," + str(y) + "\r\n"
        print(f"data_to_send: {data_to_send}")
        if not send_packet(uart_connection, data_to_send):
            uart_connection = None
            continue
        time.sleep(0.1)