import time
import math
import board
import busio
from adafruit_lsm6ds.lsm6dsox import LSM6DSOX

import adafruit_ble
from adafruit_ble.advertising.standard import ProvideServicesAdvertisement
from adafruit_ble.services.nordic import UARTService

# Sensor setup
i2c = busio.I2C(board.SCL, board.SDA)
sensor = LSM6DSOX(i2c)

# BLE setup
ble = adafruit_ble.BLERadio()
uart = UARTService()
advertisement = ProvideServicesAdvertisement(uart)
ble.start_advertising(advertisement)

print("Waiting for Bluetooth connection...")
while not ble.connected:
    time.sleep(0.1)
print("Bluetooth connected!")


def get_rpm_and_angle():
    gx, gy, gz = sensor.gyro
    rpm_x = (gx / 360.0) * 60.0 * 60.0
    rpm_y = (gy / 360.0) * 60.0 * 60.0
    rpm_z = (gz / 360.0) * 60.0 * 60.0
    rpm_total = math.sqrt(rpm_x ** 2 + rpm_y ** 2 + rpm_z ** 2)

    ax, ay, az = sensor.acceleration
    total_accel = math.sqrt(ax ** 2 + ay ** 2 + az ** 2)
    tilt_angle = math.degrees(math.acos(az / total_accel)) if total_accel != 0 else 0

    return rpm_total, tilt_angle


while ble.connected:
    if uart.in_waiting:
        raw = uart.readline()
        if raw is not None:
            try:
                command = raw.decode("utf-8").strip()
                print("Received raw:", command)

                if command == "1":
                    uart.write(b"Starting 3-second   recording...\n")
                    print("Recording started")

                    max_rpm = 0
                    best_angle = 0

                    start_time = time.monotonic()
                    while time.monotonic() - start_time < 3:
                        rpm, angle = get_rpm_and_angle()
                        if rpm > max_rpm:
                            max_rpm = rpm
                            best_angle = angle
                        time.sleep(0.05)

                    result = ("Max RPM: {:.2f}       Angle: {:.2f}°\n").format(max_rpm, best_angle)
                    uart.write(result.encode("utf-8"))
                    print(result)
            except Exception as e:
                print("Decode error:", e)