# V005.py  (VERSION: QT-ORDER-FIX-1)
print("=== V005 VERSION: QT-ORDER-FIX-1 ===")

import sys

# ✅ MUST create QApplication BEFORE any QWidget exists
from PyQt5 import QtWidgets, QtCore
app = QtWidgets.QApplication.instance()
if app is None:
    app = QtWidgets.QApplication(sys.argv)

# Now it is safe to import pyqtgraph / OpenGL stuff
import asyncio
import math
import time
import threading
import numpy as np

import pyqtgraph.opengl as gl
from bleak import BleakClient

# ====== HARD-CODED DEVICE ======
ADDRESS = "10:51:DB:1B:E7:1E"
CHAR_UUID = "7e2a1002-1111-2222-3333-444455556666"

latest = {"ax": 0.0, "ay": 0.0, "az": 1.0, "ts": 0.0, "count": 0, "raw": ""}

def parse_csv_triplet(s: str):
    s = s.strip().strip('"')
    parts = s.split(",")
    if len(parts) != 3:
        return None
    try:
        return float(parts[0]), float(parts[1]), float(parts[2])
    except ValueError:
        return None

def notify_handler(_sender: int, data: bytearray):
    s = data.decode(errors="ignore").strip()
    latest["raw"] = s
    v = parse_csv_triplet(s)
    if v is None:
        return
    latest["ax"], latest["ay"], latest["az"] = v
    latest["ts"] = time.time()
    latest["count"] += 1

def roll_pitch_from_accel(ax, ay, az):
    roll = math.degrees(math.atan2(ay, az))
    pitch = math.degrees(math.atan2(-ax, math.sqrt(ay * ay + az * az)))
    return roll, pitch

async def ble_loop():
    while True:
        try:
            print(f"Connecting to {ADDRESS} ...")
            async with BleakClient(ADDRESS, timeout=15.0) as client:
                if not client.is_connected:
                    raise RuntimeError("Connect failed.")
                print("Connected. Enabling notifications...")
                await client.start_notify(CHAR_UUID, notify_handler)
                print("Notify enabled. Streaming...\n")
                while True:
                    await asyncio.sleep(1.0)
        except Exception as e:
            print(f"[BLE] Error/disconnect: {e}")
            print("Reconnecting in 2 seconds...\n")
            await asyncio.sleep(2.0)

def start_ble_thread():
    t = threading.Thread(target=lambda: asyncio.run(ble_loop()), daemon=True)
    t.start()

def make_cuboid(size=(2.0, 1.2, 0.1)):
    L, W, T = size
    x, y, z = L/2, W/2, T/2
    verts = np.array([
        [-x,-y,-z], [ x,-y,-z], [ x, y,-z], [-x, y,-z],
        [-x,-y, z], [ x,-y, z], [ x, y, z], [-x, y, z],
    ], dtype=float)
    faces = np.array([
        [0,1,2],[0,2,3],
        [4,6,5],[4,7,6],
        [0,4,5],[0,5,1],
        [1,5,6],[1,6,2],
        [2,6,7],[2,7,3],
        [3,7,4],[3,4,0],
    ], dtype=int)
    return verts, faces

class Viewer(QtWidgets.QMainWindow):
    def __init__(self):
        super().__init__()
        self.setWindowTitle("ADXL345 – 3D PCB Tilt (BLE)")

        self.view = gl.GLViewWidget()
        self.view.setCameraPosition(distance=6, elevation=25, azimuth=40)
        self.setCentralWidget(self.view)

        grid = gl.GLGridItem()
        grid.setSize(10, 10)
        grid.setSpacing(1, 1)
        self.view.addItem(grid)

        axis = gl.GLAxisItem()
        axis.setSize(2, 2, 2)
        self.view.addItem(axis)

        verts, faces = make_cuboid()
        colors = np.ones((faces.shape[0], 4), dtype=float)
        colors[:,0]=0.25; colors[:,1]=0.7; colors[:,2]=0.35; colors[:,3]=0.9

        meshdata = gl.MeshData(vertexes=verts, faces=faces)
        self.mesh = gl.GLMeshItem(meshdata=meshdata, faceColors=colors, drawEdges=True, edgeColor=(0,0,0,1))
        self.view.addItem(self.mesh)

        self.vec = gl.GLLinePlotItem(pos=np.array([[0,0,0],[0,0,1]]), width=3, antialias=True)
        self.view.addItem(self.vec)

        self.label = QtWidgets.QLabel(self)
        self.label.setStyleSheet("color:white; background:rgba(0,0,0,140); padding:6px;")
        self.label.move(10, 10)
        self.label.resize(440, 90)

        self.roll = 0.0
        self.pitch = 0.0

        self.timer = QtCore.QTimer()
        self.timer.timeout.connect(self.update_scene)
        self.timer.start(20)

    def update_scene(self):
        axg, ayg, azg = latest["ax"], latest["ay"], latest["az"]
        roll, pitch = roll_pitch_from_accel(axg, ayg, azg)

        alpha = 0.15
        self.roll = (1-alpha)*self.roll + alpha*roll
        self.pitch = (1-alpha)*self.pitch + alpha*pitch

        self.mesh.resetTransform()
        self.mesh.rotate(self.pitch, 0, 1, 0)
        self.mesh.rotate(self.roll, 1, 0, 0)

        self.vec.setData(pos=np.array([[0,0,0],[axg, ayg, azg]], dtype=float))

        age = (time.time()-latest["ts"])*1000 if latest["ts"] else 0
        self.label.setText(
            f"Packets: {latest['count']}   Age: {age:.0f} ms\n"
            f"ax={axg:+.3f}g  ay={ayg:+.3f}g  az={azg:+.3f}g\n"
            f"roll={self.roll:+.1f}°  pitch={self.pitch:+.1f}°\n"
            f"raw: {latest['raw']}"
        )

def main():
    print("IMPORTANT: Disconnect nRF Connect (or any other BLE client) first.")
    start_ble_thread()

    win = Viewer()
    win.resize(900, 650)
    win.show()

    sys.exit(app.exec_())

if __name__ == "__main__":
    main()