From cb17e4fccfba77040ec81e06fa3777240074397d Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 9 May 2025 15:35:50 +0200
Subject: [PATCH 01/64] added gyro sensor

---
 src/Qbead.h | 51 +++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 47 insertions(+), 4 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 915c258..160a4bb 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -32,6 +32,8 @@ const uint8_t QB_UUID_SPH_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
 const uint8_t QB_UUID_ACC_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
+const uint8_t QB_UUID_GYR_CHAR[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
 
 const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 
@@ -135,7 +137,8 @@ class Qbead {
         bleservice(QB_UUID_SERVICE),
         blecharcol(QB_UUID_COL_CHAR),
         blecharsph(QB_UUID_SPH_CHAR),
-        blecharacc(QB_UUID_ACC_CHAR)
+        blecharacc(QB_UUID_ACC_CHAR),
+        blechargyr(QB_UUID_GYR_CHAR)
         {}
 
   static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
@@ -147,6 +150,7 @@ class Qbead {
   BLECharacteristic blecharcol;
   BLECharacteristic blecharsph;
   BLECharacteristic blecharacc;
+  BLECharacteristic blechargyr;
   uint8_t connection_count = 0;
 
   const uint8_t nsections;
@@ -155,8 +159,9 @@ class Qbead {
   const uint8_t phi_quant;
   const uint8_t ix, iy, iz;
   const bool sx, sy, sz;
-  float rbuffer[3];
+  float rbuffer[3], rgyrobuffer[3];
   float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
+  float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro // filtered and raw gyro measurements, in units of deg/s
   float t_acc, p_acc;        // theta and phi according to gravity
   float T_imu;             // last update from the IMU
 
@@ -217,13 +222,20 @@ class Qbead {
     blecharsph.setWriteCallback(ble_callback_theta_phi);
     blecharsph.begin();
     blecharsph.write(zerobuffer20, 2);
-    // BLE Characteristic IMU xyz readout
+    // BLE Characteristic IMU xyz accelerometer readout
     blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
     blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN);
     blecharacc.setUserDescriptor("xyz acceleration");
     blecharacc.setFixedLen(3*sizeof(float));
     blecharacc.begin();
     blecharacc.write(zerobuffer20, 3*sizeof(float));
+    // BLE Characteristic IMU xyz gyroscope readout
+    blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
+    blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blechargyr.setUserDescriptor("xyz gyroscope");
+    blechargyr.setFixedLen(3*sizeof(float));
+    blechargyr.begin();
+    blechargyr.write(zerobuffer20, 3*sizeof(float));
     startBLEadv();
   }
 
@@ -294,13 +306,23 @@ class Qbead {
     rbuffer[0] = imu.readFloatAccelX();
     rbuffer[1] = imu.readFloatAccelY();
     rbuffer[2] = imu.readFloatAccelZ();
+    rgyrobuffer[0] = imu.readFloatGyroX();
+    rgyrobuffer[1] = imu.readFloatGyroY();
+    rgyrobuffer[2] = imu.readFloatGyroZ();
+
+    // calibration of imu because imu is not aligned with bloch sphere
     rx = (1-2*sx)*rbuffer[ix];
     ry = (1-2*sy)*rbuffer[iy];
     rz = (1-2*sz)*rbuffer[iz];
 
+    rxGyro = (1-2*sx)*rgyrobuffer[ix];
+    ryGyro = (1-2*sy)*rgyrobuffer[iy];
+    rzGyro = (1-2*sz)*rgyrobuffer[iz];
+
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
+    // accelerometer filter
     const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable
     if (delta > 100000) {
       x = rx;
@@ -312,6 +334,10 @@ class Qbead {
       y = d*ry+(1-d)*y;
       z = d*rz+(1-d)*z;
     }
+    //Gyroscope filter(currently no filter) // TODO make a better filter for gyroscope
+    xGyro = rxGyro;
+    yGyro = ryGyro;
+    zGyro = rzGyro;
 
     t_acc = theta(x, y, z)*180/3.14159;
     p_acc = phi(x, y)*180/3.14159;
@@ -329,12 +355,21 @@ class Qbead {
       Serial.print(p_acc);
       Serial.print("\t-360\t360\t");
       Serial.println();
+      Serial.print(xGyro);
+      Serial.print("\t");
+      Serial.print(yGyro);
+      Serial.print("\t");
+      Serial.print(zGyro);
+      Serial.println();
     }
 
     rbuffer[0] = x;
     rbuffer[1] = y;
     rbuffer[2] = z;
-    blecharacc.write(rbuffer, 3*sizeof(float));
+    rgyrobuffer[0] = xGyro;
+    rgyrobuffer[1] = yGyro;
+    rgyrobuffer[2] = zGyro;
+    blecharacc.write(rgyrobuffer, 3*sizeof(float));
     for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
     {
       if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) )
@@ -342,6 +377,14 @@ class Qbead {
         blecharacc.notify(rbuffer, 3*sizeof(float));
       }
     }
+    blechargyr.write(rgyrobuffer, 3*sizeof(float));
+    for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
+    {
+      if ( Bluefruit.connected(conn_hdl) && blechargyr.notifyEnabled(conn_hdl) )
+      {
+        blechargyr.notify(rbuffer, 3*sizeof(float));
+      }
+    }
   }
 
   void startBLEadv(void)

From 7cfb301e8f3940253a8a8bc59f516630ae31633a Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 9 May 2025 15:51:57 +0200
Subject: [PATCH 02/64] added fixes from testing

---
 src/Qbead.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 160a4bb..12cb122 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -10,7 +10,7 @@
 #include <bluefruit.h>
 
 // default configs
-#define QB_LEDPIN 0
+#define QB_LEDPIN 10
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
 #define QB_NSECTIONS 6
 #define QB_NLEGS 12
@@ -161,7 +161,7 @@ class Qbead {
   const bool sx, sy, sz;
   float rbuffer[3], rgyrobuffer[3];
   float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
-  float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro // filtered and raw gyro measurements, in units of deg/s
+  float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro; // filtered and raw gyro measurements, in units of deg/s
   float t_acc, p_acc;        // theta and phi according to gravity
   float T_imu;             // last update from the IMU
 

From 56b6923adad105f31ed2df08cdb3c5753aa57cbb Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Mon, 12 May 2025 14:10:47 +0200
Subject: [PATCH 03/64] fixed write to ble blecharacc

---
 src/Qbead.h | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 12cb122..fc6812d 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -369,7 +369,8 @@ class Qbead {
     rgyrobuffer[0] = xGyro;
     rgyrobuffer[1] = yGyro;
     rgyrobuffer[2] = zGyro;
-    blecharacc.write(rgyrobuffer, 3*sizeof(float));
+
+    blecharacc.write(rbuffer, 3*sizeof(float));
     for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
     {
       if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) )
@@ -382,7 +383,7 @@ class Qbead {
     {
       if ( Bluefruit.connected(conn_hdl) && blechargyr.notifyEnabled(conn_hdl) )
       {
-        blechargyr.notify(rbuffer, 3*sizeof(float));
+        blechargyr.notify(rgyrobuffer, 3*sizeof(float));
       }
     }
   }

From 4c363b0724e923905e9ac872bb8fed617536944c Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Thu, 15 May 2025 11:50:44 +0200
Subject: [PATCH 04/64] added code (rotation around gravity) from pr #35

---
 src/Qbead.h | 433 ++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 351 insertions(+), 82 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index fc6812d..6cff170 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -6,14 +6,15 @@
 #include <Adafruit_NeoPixel.h>
 #include <LSM6DS3.h>
 #include <math.h>
+#include <ArduinoEigen.h>
 
 #include <bluefruit.h>
 
+using namespace Eigen;
+
 // default configs
 #define QB_LEDPIN 10
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
-#define QB_NSECTIONS 6
-#define QB_NLEGS 12
 #define QB_IMU_ADDR 0x6A
 #define QB_IX 0
 #define QB_IY 2
@@ -21,6 +22,8 @@
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
+#define GYRO_THRESHOLD 0.0001
+#define QB_PIXEL_COUNT 62
 
 #define QB_MAX_PRPH_CONNECTION 2
 
@@ -111,14 +114,197 @@ void connect_callback(uint16_t conn_handle)
   Serial.println(central_name);
 }
 
+// In rads
+void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
+{
+  // Normalize yaw to be between 0 and 2*PI
+  phi = fmod(phi, 2 * PI);
+  if (phi < 0)
+  {
+    phi += 2 * PI;
+  }
+  if (!checkThetaAndPhi(theta * 180 / PI, phi * 180 / PI))
+  {
+    Serial.print("Theta or Phi out of range when creating coordinates class, initializing as 1");
+    Serial.print("Theta: ");
+    Serial.print(theta);
+    Serial.print("Phi: ");
+    Serial.println(phi);
+    x = 0;
+    y = 0;
+    z = 1;
+    return;
+  }
+
+  x = sin(theta) * cos(phi);
+  y = sin(theta) * sin(phi);
+  z = cos(theta);
+}
+
 namespace Qbead {
 
+class Coordinates
+{
+public:
+  Vector3d v;
+
+  Coordinates(float argx, float argy, float argz)
+  {
+    v = Vector3d(argx, argy, argz);
+    v.normalize();
+  }
+
+  // In rads
+  Coordinates(float theta, float phi)
+  {
+    float x, y, z = 0;
+    sphericalToCartesian(theta, phi, x, y, z);
+    v = Vector3d(x, y, z);
+  }
+
+  Coordinates(Vector3d vector)
+  {
+    v = vector;
+    v.normalize();
+  }
+
+  float x()
+  {
+    return v(0);
+  }
+
+  float y()
+  {
+    return v(1);
+  }
+
+  float z()
+  {
+    return v(2);
+  }
+
+  // In rads
+  float theta()
+  {
+    return acos(z());
+  }
+
+  // In rads
+  float phi()
+  {
+    return atan2(y(), x());
+  }
+
+  float dist(Vector3d other) const
+  {
+    Vector3d diff = v - other;
+    return diff.norm();
+  }
+
+  void set(float argx, float argy, float argz)
+  {
+    v = Vector3d(argx, argy, argz);
+    v.normalize();
+  }
+
+  // in rads
+  void set(float theta, float phi)
+  {
+    float x, y, z = 0;
+    sphericalToCartesian(theta, phi, x, y, z);
+    v = Vector3d(x, y, z);
+  }
+
+  void set(Vector3d vector) {
+    v = vector;
+    v.normalize();
+  }
+
+  // in rads
+  void setTheta(float theta)
+  {
+    set(theta, phi());
+  }
+
+  // in rads
+  void setPhi(float phi)
+  {
+    set(theta(), phi);
+  }
+
+  // This can be used to align the internal z-axis with gravity
+  Coordinates rotateRelativeTo(Vector3d other)
+  {
+    Vector3d ref = Vector3d(0, 0, -1);
+
+    // Normal case: Compute the axis and angle for rotation
+    Vector3d axis = other.cross(ref).normalized();
+    float angle = acos(other.dot(ref));
+
+    // Apply the rotation
+    AngleAxisd rotation = AngleAxisd(angle, axis);
+    Vector3d rotated = rotation * v;
+    return Coordinates(rotated);
+  }
+};
+
+class QuantumState
+{
+private:
+  Coordinates stateCoordinates;
+
+public:
+  QuantumState(Coordinates argStateCoordinates) : stateCoordinates(argStateCoordinates) {}
+  QuantumState() : stateCoordinates(0, 0, 1) {}
+
+  void setCoordinates(Coordinates argStateCoordinates)
+  {
+    stateCoordinates.set(argStateCoordinates.v);
+  }
+
+  Coordinates getCoordinates()
+  {
+    return stateCoordinates;
+  }
+
+  int collapse()
+  {
+    const float theta = stateCoordinates.theta();
+    const float a = cos(theta / 2);
+    const bool is1 = random(0, 100) < a * a * 100;
+    this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
+    return is1 ? 1 : 0;
+  }
+
+  // Rotate PI around the x axis
+  void gateX()
+  {
+    stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
+  }
+
+  // Rotate PI around the y axis
+  void gateZ()
+  {
+    stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
+  }
+
+  // Rotate PI around the z axis
+  void gateY()
+  {
+    stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
+  }
+
+  // Rotate PI around the xz axis
+  void gateH()
+  {
+    stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
+  }
+};
+
 class Qbead {
 public:
   Qbead(const uint16_t pin00 = QB_LEDPIN,
         const uint16_t pixelconfig = QB_PIXELCONFIG,
-        const uint16_t nsections = QB_NSECTIONS,
-        const uint16_t nlegs = QB_NLEGS,
         const uint8_t imu_addr = QB_IMU_ADDR,
         const uint8_t ix = QB_IX,
         const uint8_t iy = QB_IY,
@@ -127,11 +313,7 @@ class Qbead {
         const bool sy = QB_SY,
         const bool sz = QB_SZ)
       : imu(LSM6DS3(I2C_MODE, imu_addr)),
-        pixels(Adafruit_NeoPixel(nlegs * (nsections - 1) + 2, pin00, pixelconfig)),
-        nsections(nsections),
-        nlegs(nlegs),
-        theta_quant(180 / nsections),
-        phi_quant(360 / nlegs),
+        pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)),
         ix(ix), iy(iy), iz(iz),
         sx(sx), sy(sy), sz(sz),
         bleservice(QB_UUID_SERVICE),
@@ -153,21 +335,85 @@ class Qbead {
   BLECharacteristic blechargyr;
   uint8_t connection_count = 0;
 
-  const uint8_t nsections;
-  const uint8_t nlegs;
-  const uint8_t theta_quant;
-  const uint8_t phi_quant;
   const uint8_t ix, iy, iz;
   const bool sx, sy, sz;
   float rbuffer[3], rgyrobuffer[3];
   float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
   float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro; // filtered and raw gyro measurements, in units of deg/s
-  float t_acc, p_acc;        // theta and phi according to gravity
   float T_imu;             // last update from the IMU
+  Coordinates gravity = Coordinates(0, 0, 1); // gravity vector
+  float yaw;
 
   float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble = 0xffffff; // color as sent over BLE connection
 
+  // led map index to Coordinates
+  // This map is for the first version of the pcb
+  Coordinates led_map_v1[62] = {
+    Coordinates(PI, 0),
+    Coordinates(5 * PI / 6, 9 * PI / 6),
+    Coordinates(4 * PI / 6, 9 * PI / 6),
+    Coordinates(3 * PI / 6, 9 * PI / 6),
+    Coordinates(2 * PI / 6, 9 * PI / 6),
+    Coordinates(PI / 6, 9 * PI / 6),
+    Coordinates(0, 0),
+    Coordinates(5 * PI / 6, 10 * PI / 6),
+    Coordinates(4 * PI / 6, 10 * PI / 6),
+    Coordinates(3 * PI / 6, 10 * PI / 6),
+    Coordinates(2 * PI / 6, 10 * PI / 6),
+    Coordinates(PI / 6, 10 * PI / 6),
+    Coordinates(5 * PI / 6, 11 * PI / 6),
+    Coordinates(4 * PI / 6, 11 * PI / 6),
+    Coordinates(3 * PI / 6, 11 * PI / 6),
+    Coordinates(2 * PI / 6, 11 * PI / 6),
+    Coordinates(PI / 6, 11 * PI / 6),
+    Coordinates(5 * PI / 6, 0),
+    Coordinates(4 * PI / 6, 0),
+    Coordinates(3 * PI / 6, 0),
+    Coordinates(2 * PI / 6, 0),
+    Coordinates(PI / 6, 0),
+    Coordinates(5 * PI / 6, PI / 6),
+    Coordinates(4 * PI / 6, PI / 6),
+    Coordinates(3 * PI / 6, PI / 6),
+    Coordinates(2 * PI / 6, PI / 6),
+    Coordinates(PI / 6, PI / 6),
+    Coordinates(5 * PI / 6, 2 * PI / 6),
+    Coordinates(4 * PI / 6, 2 * PI / 6),
+    Coordinates(3 * PI / 6, 2 * PI / 6),
+    Coordinates(2 * PI / 6, 2 * PI / 6),
+    Coordinates(PI / 6, 2 * PI / 6),
+    Coordinates(5 * PI / 6, 3 * PI / 6),
+    Coordinates(4 * PI / 6, 3 * PI / 6),
+    Coordinates(3 * PI / 6, 3 * PI / 6),
+    Coordinates(2 * PI / 6, 3 * PI / 6),
+    Coordinates(PI / 6, 3 * PI / 6),
+    Coordinates(5 * PI / 6, 4 * PI / 6),
+    Coordinates(4 * PI / 6, 4 * PI / 6),
+    Coordinates(3 * PI / 6, 4 * PI / 6),
+    Coordinates(2 * PI / 6, 4 * PI / 6),
+    Coordinates(PI / 6, 4 * PI / 6),
+    Coordinates(5 * PI / 6, 5 * PI / 6),
+    Coordinates(4 * PI / 6, 5 * PI / 6),
+    Coordinates(3 * PI / 6, 5 * PI / 6),
+    Coordinates(2 * PI / 6, 5 * PI / 6),
+    Coordinates(PI / 6, 5 * PI / 6),
+    Coordinates(5 * PI / 6, 6 * PI / 6),
+    Coordinates(4 * PI / 6, 6 * PI / 6),
+    Coordinates(3 * PI / 6, 6 * PI / 6),
+    Coordinates(2 * PI / 6, 6 * PI / 6),
+    Coordinates(PI / 6, 6 * PI / 6),
+    Coordinates(5 * PI / 6, 7 * PI / 6),
+    Coordinates(4 * PI / 6, 7 * PI / 6),
+    Coordinates(3 * PI / 6, 7 * PI / 6),
+    Coordinates(2 * PI / 6, 7 * PI / 6),
+    Coordinates(PI / 6, 7 * PI / 6),
+    Coordinates(5 * PI / 6, 8 * PI / 6),
+    Coordinates(4 * PI / 6, 8 * PI / 6),
+    Coordinates(3 * PI / 6, 8 * PI / 6),
+    Coordinates(2 * PI / 6, 8 * PI / 6),
+    Coordinates(PI / 6, 8 * PI / 6),
+  };
+
   static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
     Serial.println("[INFO]{BLE} Received a write on the color characteristic");
     singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
@@ -228,7 +474,7 @@ class Qbead {
     blecharacc.setUserDescriptor("xyz acceleration");
     blecharacc.setFixedLen(3*sizeof(float));
     blecharacc.begin();
-    blecharacc.write(zerobuffer20, 3*sizeof(float));
+    blecharacc.write(zerobuffer20, 3*sizeof(float));    
     // BLE Characteristic IMU xyz gyroscope readout
     blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
     blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN);
@@ -247,65 +493,87 @@ class Qbead {
     pixels.show();
   }
 
-  void setLegPixelColor(int leg, int pixel, uint32_t color) {
-    leg = nlegs - leg; // invert direction for the phi angle, because the PCB is set up as a left-handed coordinate system
-    leg = leg % nlegs;
-    if (leg == 0) {
-      pixels.setPixelColor(pixel, color);
-    } else if (pixel == 0) {
-      pixels.setPixelColor(0, color);
-    } else if (pixel == 6) {
-      pixels.setPixelColor(6, color);
-    } else {
-      pixels.setPixelColor(7 + (leg - 1) * (nsections - 1) + pixel - 1, color);
-    }
-  }
-
   void setBrightness(uint8_t b) {
     pixels.setBrightness(b);
   }
 
-  void setBloch_deg(float theta, float phi, uint32_t color) {
-    if (!checkThetaAndPhi(theta, phi)) return;
-    float theta_section = theta / theta_quant;
-    if (theta_section < 0.5) {
-      setLegPixelColor(0, 0, color);
-    } else if (theta_section > nsections - 0.5) {
-      setLegPixelColor(0, nsections, color);
-    } else {
-      int theta_int = min(nsections - 1, round(theta_section)); // to avoid precision issues near the end of the range
-      int phi_int = round(phi / phi_quant);
-      phi_int = phi_int > nlegs - 1 ? 0 : phi_int;
-      setLegPixelColor(phi_int, theta_int, color);
+  Coordinates getRelativeCoordinates(Coordinates c) {
+    c.setPhi(c.phi() + yaw);
+    if (gravity.x() > -0.95 && gravity.z() < 0.95) {
+      c.setPhi(c.phi() + gravity.phi());
     }
+    return c.rotateRelativeTo(gravity.v);
   }
 
-  void setBloch_deg_smooth(float theta, float phi, uint32_t c) {
-    if (!checkThetaAndPhi(theta, phi)) return;
-    float theta_section = theta / theta_quant;
-    int theta_int = min(nsections - 1, round(theta_section)); // to avoid precision issues near the end of the range
-    int phi_int = round(phi / phi_quant);
-    phi_int = phi_int > nlegs - 1 ? 0 : phi_int;
+  void setLed(Coordinates coordinates, uint32_t color, bool smooth = false) {
+    Coordinates adjusted = getRelativeCoordinates(coordinates);
+    float theta = adjusted.theta() * 180 / PI;
+    float phi = adjusted.phi() * 180 / PI;
+    if (phi < 0) {
+      phi += 360;
+    }
+    setBloch_deg(theta, phi, color, smooth);
+  }
 
-    float p = (theta_section - theta_int);
-    int theta_direction = sign(p);
-    p = abs(p);
-    float q = 1 - p;
-    p = p * p;
-    q = q * q;
+  void showAxis() {
+    setLed(Coordinates(0, 0, 1), color(255, 0, 0));
+    setLed(Coordinates(0, 0, -1), color(255, 0, 0));
+    setLed(Coordinates(0, 1, 0), color(0, 255, 0));
+    setLed(Coordinates(0, -1, 0), color(0, 255, 0));
+    setLed(Coordinates(1, 0, 0), color(0, 0, 255));
+    setLed(Coordinates(-1, 0, 0), color(0, 0, 255));
+  }
 
-    uint8_t rc = redch(c);
-    uint8_t bc = bluech(c);
-    uint8_t gc = greench(c);
+  // in rads
+  float getDistToLed(float theta, float phi, int index) {
+    const Coordinates led = led_map_v1[index];
+    const Coordinates reference(theta, phi);
+    return led.dist(reference.v);
+  }
 
-    setLegPixelColor(phi_int, theta_int, color(q * rc, q * bc, q * gc));
-    setLegPixelColor(phi_int, theta_int + theta_direction, color(p * rc, p * bc, p * gc));
+  // Single bit is lit up on the Bloch sphere  
+  void setBloch_deg(float theta, float phi, uint32_t c, bool smooth = false) {
+    int closest_index = -1;
+    float closest_dist = 1000;
+    int second_closest_index = -1;
+    float second_closest_dist = 1000;
+    for (int i = 0; i < 62; i++) {
+      float dist = getDistToLed(theta * PI / 180, phi * PI / 180, i);
+      if (dist < closest_dist) {
+        second_closest_index = closest_index;
+        second_closest_dist = closest_dist;
+        closest_index = i;
+        closest_dist = dist;
+      } else if (dist < second_closest_dist) {
+        second_closest_index = i;
+        second_closest_dist = dist;
+      }
+    }
+    if (smooth) {
+      float dist1 = getDistToLed(theta * PI / 180, phi * PI / 180, closest_index);
+      float dist2 = getDistToLed(theta * PI / 180, phi * PI / 180, second_closest_index);
+      float ratio1 = dist1 / (dist1 + dist2);
+      float ratio2 = dist2 / (dist1 + dist2);
+      uint8_t r = redch(c);
+      uint8_t g = greench(c);
+      uint8_t b = bluech(c);
+      float p1 = ratio1 * ratio1;
+      float p2 = ratio2 * ratio2;
+      pixels.setPixelColor(closest_index, color(p2 * r, p2 * g, p2 * b));
+      pixels.setPixelColor(second_closest_index, color(p1 * r, p1 * g, p1 * b));
+    } else {
+      pixels.setPixelColor(closest_index, c);
+    }
+  }
+
+  void setBloch_deg_smooth(float theta, float phi, uint32_t c) {
+    setBloch_deg(theta, phi, c, true);
   }
 
   void readIMU(bool print=true) {
     rbuffer[0] = imu.readFloatAccelX();
     rbuffer[1] = imu.readFloatAccelY();
-    rbuffer[2] = imu.readFloatAccelZ();
+    rbuffer[2] = imu.readFloatAccelZ();    
     rgyrobuffer[0] = imu.readFloatGyroX();
     rgyrobuffer[1] = imu.readFloatGyroY();
     rgyrobuffer[2] = imu.readFloatGyroZ();
@@ -315,6 +583,7 @@ class Qbead {
     ry = (1-2*sy)*rbuffer[iy];
     rz = (1-2*sz)*rbuffer[iz];
 
+    // calibration of imu because imu is not aligned with bloch sphere
     rxGyro = (1-2*sx)*rgyrobuffer[ix];
     ryGyro = (1-2*sy)*rgyrobuffer[iy];
     rzGyro = (1-2*sz)*rgyrobuffer[iz];
@@ -322,7 +591,6 @@ class Qbead {
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
-    // accelerometer filter
     const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable
     if (delta > 100000) {
       x = rx;
@@ -334,15 +602,21 @@ class Qbead {
       y = d*ry+(1-d)*y;
       z = d*rz+(1-d)*z;
     }
-    //Gyroscope filter(currently no filter) // TODO make a better filter for gyroscope
-    xGyro = rxGyro;
-    yGyro = ryGyro;
-    zGyro = rzGyro;
-
-    t_acc = theta(x, y, z)*180/3.14159;
-    p_acc = phi(x, y)*180/3.14159;
-    if (p_acc<0) {p_acc+=360;}// to bring it to [0,360] range
 
+    // Gyroscope
+    xGyro = rxGyro * delta * PI / (1000000 * 180);
+    yGyro = ryGyro * delta * PI / (1000000 * 180);
+    // The zGyro has an offset of 0.0006 rad/s
+    zGyro = rzGyro * delta * PI / (1000000 * 180) - 0.0006;
+    if (xGyro < GYRO_THRESHOLD && xGyro > -GYRO_THRESHOLD) xGyro = 0;
+    if (yGyro < GYRO_THRESHOLD && yGyro > -GYRO_THRESHOLD) yGyro = 0;
+    if (zGyro < GYRO_THRESHOLD && zGyro > -GYRO_THRESHOLD) zGyro = 0;
+
+    gravity = Coordinates(x, y, z);
+    Vector3d gyro = Vector3d(xGyro, yGyro, zGyro);
+    yaw += gravity.v.dot(gyro);
+    yaw = fmod(yaw, 2 * PI);
+    
     if (print) {
       Serial.print(x);
       Serial.print("\t");
@@ -350,26 +624,18 @@ class Qbead {
       Serial.print("\t");
       Serial.print(z);
       Serial.print("\t-1\t1\t");
-      Serial.print(t_acc);
+      Serial.print(xGyro * 1000);
       Serial.print("\t");
-      Serial.print(p_acc);
-      Serial.print("\t-360\t360\t");
-      Serial.println();
-      Serial.print(xGyro);
+      Serial.print(yGyro * 1000);
       Serial.print("\t");
-      Serial.print(yGyro);
+      Serial.print(zGyro * 1000);
       Serial.print("\t");
-      Serial.print(zGyro);
-      Serial.println();
+      Serial.println(yaw * 180 / PI);
     }
 
     rbuffer[0] = x;
     rbuffer[1] = y;
     rbuffer[2] = z;
-    rgyrobuffer[0] = xGyro;
-    rgyrobuffer[1] = yGyro;
-    rgyrobuffer[2] = zGyro;
-
     blecharacc.write(rbuffer, 3*sizeof(float));
     for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
     {
@@ -378,12 +644,15 @@ class Qbead {
         blecharacc.notify(rbuffer, 3*sizeof(float));
       }
     }
+    rgyrobuffer[0] = xGyro;
+    rgyrobuffer[1] = yGyro;
+    rgyrobuffer[2] = zGyro;
     blechargyr.write(rgyrobuffer, 3*sizeof(float));
     for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
     {
       if ( Bluefruit.connected(conn_hdl) && blechargyr.notifyEnabled(conn_hdl) )
       {
-        blechargyr.notify(rgyrobuffer, 3*sizeof(float));
+        blechargyr.notify(rbuffer, 3*sizeof(float));
       }
     }
   }
@@ -423,4 +692,4 @@ Qbead *Qbead::singletoninstance = nullptr;
 
 } // end namespace
 
-#endif // QBEAD_H
+#endif // QBEAD_H
\ No newline at end of file

From 72091aa7c2a87c1df25d69beee0c4062f27e0648 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Thu, 15 May 2025 12:05:26 +0200
Subject: [PATCH 05/64] rotation detection and gate execution

---
 src/Qbead.h | 83 +++++++++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 78 insertions(+), 5 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 6cff170..4a71049 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -146,7 +146,7 @@ namespace Qbead {
 class Coordinates
 {
 public:
-  Vector3d v;
+Vector3d v;
 
   Coordinates(float argx, float argy, float argz)
   {
@@ -337,7 +337,7 @@ class Qbead {
 
   const uint8_t ix, iy, iz;
   const bool sx, sy, sz;
-  float rbuffer[3], rgyrobuffer[3];
+  float rbuffer[3], rgyrobuffer[3], filteredGyro[3], rotatedGyro[3];
   float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
   float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro; // filtered and raw gyro measurements, in units of deg/s
   float T_imu;             // last update from the IMU
@@ -570,6 +570,49 @@ class Qbead {
     setBloch_deg(theta, phi, c, true);
   }
 
+  void rotateGyroAroundGravity()
+  {
+    Coordinates rotatedX = getRelativeCoordinates(Coordinates(1, 0, 0));
+    Coordinates rotatedY = getRelativeCoordinates(Coordinates(0, 1, 0));
+    Coordinates rotatedZ = getRelativeCoordinates(Coordinates(0, 0, 1));
+
+    rotatedGyro[0] = rotatedX.x()*filteredGyro[0] + rotatedX.y()*filteredGyro[1] + rotatedX.z()*filteredGyro[2];
+    rotatedGyro[1] = rotatedY.x()*filteredGyro[0] + rotatedY.y()*filteredGyro[1] + rotatedY.z()*filteredGyro[2];
+    rotatedGyro[2] = rotatedZ.x()*filteredGyro[0] + rotatedZ.y()*filteredGyro[1] + rotatedZ.z()*filteredGyro[2];
+  }
+
+  bool checkRotation(QuantumState &toBeRotated)
+  {
+    if (abs(rotatedGyro[0]) > 300)
+    {
+      if (Serial)
+      {
+        Serial.println("Executing X gate");
+      }
+      toBeRotated.gateX();
+      return true;
+    }
+    if (abs(rotatedGyro[1]) > 300)
+    {
+      if (Serial)
+      {
+        Serial.println("Executing Y gate");
+      }
+      toBeRotated.gateY();
+      return true;
+    }
+    if (abs(rotatedGyro[2]) > 300)
+    {
+      if (Serial)
+      {
+        Serial.println("Executing Z gate");
+      }
+      toBeRotated.gateZ();
+      return true;
+    }
+    return false;
+  }
+
   void readIMU(bool print=true) {
     rbuffer[0] = imu.readFloatAccelX();
     rbuffer[1] = imu.readFloatAccelY();
@@ -588,16 +631,37 @@ class Qbead {
     ryGyro = (1-2*sy)*rgyrobuffer[iy];
     rzGyro = (1-2*sz)*rgyrobuffer[iz];
 
+    Serial.print("raw gyro: ");
+    Serial.print(rxGyro);
+    Serial.print("\t");
+    Serial.print(ryGyro);
+    Serial.print("\t");
+    Serial.println(rzGyro);
+    
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
-    const float T = 100000; // 100 ms // TODO make the filter timeconstant configurable
+    // timefilter filter
+    const float T_acc = 100000; // 100 ms // TODO make the filter timeconstant configurable
+    const float T_gyr = 10000; // 10 ms
+    if (delta > 10000)
+    {
+      filteredGyro[0] = rxGyro;
+      filteredGyro[1] = ryGyro;
+      filteredGyro[2] = rzGyro;
+    } else
+    {
+      float d = delta/T_gyr;
+      filteredGyro[0] = d*rxGyro+(1-d)*filteredGyro[0];
+      filteredGyro[1] = d*ryGyro+(1-d)*filteredGyro[1];
+      filteredGyro[2] = d*rzGyro+(1-d)*filteredGyro[2];
+    }
     if (delta > 100000) {
       x = rx;
       y = ry;
       z = rz;
     } else {
-      float d = delta/T;
+      float d = delta/T_acc;
       x = d*rx+(1-d)*x;
       y = d*ry+(1-d)*y;
       z = d*rz+(1-d)*z;
@@ -616,8 +680,17 @@ class Qbead {
     Vector3d gyro = Vector3d(xGyro, yGyro, zGyro);
     yaw += gravity.v.dot(gyro);
     yaw = fmod(yaw, 2 * PI);
-    
+
+    rotateGyroAroundGravity(); //Sets the rotatedgyro array
+
+
     if (print) {
+      Serial.print("rotatedGyro: ");
+      Serial.print(rotatedGyro[0]);
+      Serial.print("\t");
+      Serial.print(rotatedGyro[1]);
+      Serial.print("\t");
+      Serial.println(rotatedGyro[2]);
       Serial.print(x);
       Serial.print("\t");
       Serial.print(y);

From e4017f76c554f9b4ff47631b06d9a7e47c3cb9d6 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Thu, 15 May 2025 12:07:49 +0200
Subject: [PATCH 06/64] arduino sketch to test pauli gates

---
 .../PauliGate_detection.ino                   | 54 +++++++++++++++++++
 1 file changed, 54 insertions(+)
 create mode 100644 examples/PauliGate_detection/PauliGate_detection.ino

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
new file mode 100644
index 0000000..754a5a5
--- /dev/null
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -0,0 +1,54 @@
+#include <Qbead.h>
+
+Qbead::Qbead bead;
+Qbead::QuantumState state = Qbead::QuantumState(Qbead::Coordinates(1, 2.3));
+bool freeze = 0;
+float timeIMU = 0;
+float counter = 0;
+uint32_t cooldownColor = color(255, 255, 255);
+
+
+void setup() {
+  bead.begin();
+  bead.setBrightness(25); // way too bright
+  Serial.println("testing all pixels discretely");
+  for (int i = 0; i < bead.pixels.numPixels(); i++) {
+    bead.pixels.setPixelColor(i, color(255, 255, 255));
+    bead.pixels.show();
+    delay(5);
+  }
+  Serial.println("testing smooth transition between pixels");
+  for (int phi = 0; phi < 360; phi += 30) {
+    for (int theta = 0; theta < 180; theta += 3) {
+      bead.clear();
+      bead.setBloch_deg(theta, phi, colorWheel_deg(phi));
+      bead.show();
+    }
+  }
+  Serial.println("starting inertial tracking");
+}
+
+void loop() {
+  bead.readIMU();
+  bead.clear();
+  bead.showAxis();
+  if (freeze)
+  {
+    cooldownColor = color(255, 255, 0);
+    float newTime = micros();
+    counter += newTime - timeIMU;
+    timeIMU = newTime;
+    if (counter > 3000000)
+    {
+      counter = 0;
+      freeze = 0;
+    }
+  } else
+  {
+    cooldownColor = color(255, 255, 255);
+    freeze = bead.checkRotation(state);
+    timeIMU = micros();
+  }
+  bead.setLed(state.getCoordinates(), cooldownColor);
+  bead.show();
+}
\ No newline at end of file

From 3eec61cde928d50311b27c8e9c9cb7332f6940e5 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 16 May 2025 10:26:30 +0200
Subject: [PATCH 07/64] animation for pauli gates

---
 .../PauliGate_detection.ino                   | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 754a5a5..d9fb613 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -6,8 +6,26 @@ bool freeze = 0;
 float timeIMU = 0;
 float counter = 0;
 uint32_t cooldownColor = color(255, 255, 255);
+const bool toggleAnimationOn = 1;
 
 
+void animationGate(Qbead::Coordinates oldPoint, int steps, int animationLength)
+{
+  for (int i = 0; i < steps; i++)
+  {
+    bead.clear();
+    float q = i/ (float) steps;
+    float newTheta = q*state.getCoordinates().theta() + (1-q)*oldPoint.theta();
+    float newPhi = q*state.getCoordinates().phi() + (1-q)*oldPoint.phi();
+    Qbead::Coordinates animationPoint = Qbead::Coordinates(newTheta, newPhi);
+    bead.setLed(animationPoint, color(255, 255, 0));
+    bead.readIMU();
+    bead.showAxis();
+    bead.show();
+    delay(animationLength/steps);
+  }
+}
+
 void setup() {
   bead.begin();
   bead.setBrightness(25); // way too bright
@@ -45,9 +63,14 @@ void loop() {
     }
   } else
   {
+    Qbead::QuantumState oldState = state;
     cooldownColor = color(255, 255, 255);
     freeze = bead.checkRotation(state);
     timeIMU = micros();
+    if (freeze && toggleAnimationOn)
+    {
+      animationGate(oldState.getCoordinates(), 30, 4000);
+    }
   }
   bead.setLed(state.getCoordinates(), cooldownColor);
   bead.show();

From 76cb79b731d1ff8a3dc89811ae553e47883475eb Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 16 May 2025 10:43:46 +0200
Subject: [PATCH 08/64] Make vectors from gravity and gyro

---
 src/Qbead.h | 95 +++++++++++++----------------------------------------
 1 file changed, 23 insertions(+), 72 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 4a71049..0bc2d24 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -146,7 +146,7 @@ namespace Qbead {
 class Coordinates
 {
 public:
-Vector3d v;
+  Vector3d v;
 
   Coordinates(float argx, float argy, float argz)
   {
@@ -231,21 +231,6 @@ Vector3d v;
   {
     set(theta(), phi);
   }
-
-  // This can be used to align the internal z-axis with gravity
-  Coordinates rotateRelativeTo(Vector3d other)
-  {
-    Vector3d ref = Vector3d(0, 0, -1);
-
-    // Normal case: Compute the axis and angle for rotation
-    Vector3d axis = other.cross(ref).normalized();
-    float angle = acos(other.dot(ref));
-
-    // Apply the rotation
-    AngleAxisd rotation = AngleAxisd(angle, axis);
-    Vector3d rotated = rotation * v;
-    return Coordinates(rotated);
-  }
 };
 
 class QuantumState
@@ -337,11 +322,12 @@ class Qbead {
 
   const uint8_t ix, iy, iz;
   const bool sx, sy, sz;
-  float rbuffer[3], rgyrobuffer[3], filteredGyro[3], rotatedGyro[3];
+  float rbuffer[3], rgyrobuffer[3];
   float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
   float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro; // filtered and raw gyro measurements, in units of deg/s
   float T_imu;             // last update from the IMU
-  Coordinates gravity = Coordinates(0, 0, 1); // gravity vector
+  Vector3d gravity = Vector3d(0, 0, 1); // gravity vector
+  Vector3d gyroVector = Vector3d(0, 0, 1); // gyro vector
   float yaw;
 
   float t_ble, p_ble; // theta and phi as sent over BLE connection
@@ -497,18 +483,9 @@ class Qbead {
     pixels.setBrightness(b);
   }
 
-  Coordinates getRelativeCoordinates(Coordinates c) {
-    c.setPhi(c.phi() + yaw);
-    if (gravity.x() > -0.95 && gravity.z() < 0.95) {
-      c.setPhi(c.phi() + gravity.phi());
-    }
-    return c.rotateRelativeTo(gravity.v);
-  }
-
   void setLed(Coordinates coordinates, uint32_t color, bool smooth = false) {
-    Coordinates adjusted = getRelativeCoordinates(coordinates);
-    float theta = adjusted.theta() * 180 / PI;
-    float phi = adjusted.phi() * 180 / PI;
+    float theta = coordinates.theta() * 180 / PI;
+    float phi = coordinates.phi() * 180 / PI;
     if (phi < 0) {
       phi += 360;
     }
@@ -570,43 +547,23 @@ class Qbead {
     setBloch_deg(theta, phi, c, true);
   }
 
-  void rotateGyroAroundGravity()
-  {
-    Coordinates rotatedX = getRelativeCoordinates(Coordinates(1, 0, 0));
-    Coordinates rotatedY = getRelativeCoordinates(Coordinates(0, 1, 0));
-    Coordinates rotatedZ = getRelativeCoordinates(Coordinates(0, 0, 1));
-
-    rotatedGyro[0] = rotatedX.x()*filteredGyro[0] + rotatedX.y()*filteredGyro[1] + rotatedX.z()*filteredGyro[2];
-    rotatedGyro[1] = rotatedY.x()*filteredGyro[0] + rotatedY.y()*filteredGyro[1] + rotatedY.z()*filteredGyro[2];
-    rotatedGyro[2] = rotatedZ.x()*filteredGyro[0] + rotatedZ.y()*filteredGyro[1] + rotatedZ.z()*filteredGyro[2];
-  }
-
   bool checkRotation(QuantumState &toBeRotated)
   {
-    if (abs(rotatedGyro[0]) > 300)
+    if (abs(gyroVector[0]) > 300)
     {
-      if (Serial)
-      {
-        Serial.println("Executing X gate");
-      }
+      Serial.println("Executing X gate");
       toBeRotated.gateX();
       return true;
     }
-    if (abs(rotatedGyro[1]) > 300)
+    if (abs(gyroVector[1]) > 300)
     {
-      if (Serial)
-      {
-        Serial.println("Executing Y gate");
-      }
+      Serial.println("Executing Y gate");
       toBeRotated.gateY();
       return true;
     }
-    if (abs(rotatedGyro[2]) > 300)
+    if (abs(gyroVector[2]) > 300)
     {
-      if (Serial)
-      {
-        Serial.println("Executing Z gate");
-      }
+      Serial.println("Executing Z gate");
       toBeRotated.gateZ();
       return true;
     }
@@ -646,15 +603,12 @@ class Qbead {
     const float T_gyr = 10000; // 10 ms
     if (delta > 10000)
     {
-      filteredGyro[0] = rxGyro;
-      filteredGyro[1] = ryGyro;
-      filteredGyro[2] = rzGyro;
-    } else
+      gyroVector = Vector3d(rxGyro, ryGyro, rzGyro);
+    }
+    else
     {
-      float d = delta/T_gyr;
-      filteredGyro[0] = d*rxGyro+(1-d)*filteredGyro[0];
-      filteredGyro[1] = d*ryGyro+(1-d)*filteredGyro[1];
-      filteredGyro[2] = d*rzGyro+(1-d)*filteredGyro[2];
+      float d = delta / T_gyr;
+      gyroVector = d * Vector3d(rxGyro, ryGyro, rzGyro) + (1 - d) * gyroVector;
     }
     if (delta > 100000) {
       x = rx;
@@ -676,21 +630,18 @@ class Qbead {
     if (yGyro < GYRO_THRESHOLD && yGyro > -GYRO_THRESHOLD) yGyro = 0;
     if (zGyro < GYRO_THRESHOLD && zGyro > -GYRO_THRESHOLD) zGyro = 0;
 
-    gravity = Coordinates(x, y, z);
+    gravity = Vector3d(x, y, z);
     Vector3d gyro = Vector3d(xGyro, yGyro, zGyro);
-    yaw += gravity.v.dot(gyro);
+    yaw += gravity.dot(gyro);
     yaw = fmod(yaw, 2 * PI);
 
-    rotateGyroAroundGravity(); //Sets the rotatedgyro array
-
-
     if (print) {
-      Serial.print("rotatedGyro: ");
-      Serial.print(rotatedGyro[0]);
+      Serial.print("gyro: ");
+      Serial.print(gyroVector(0));
       Serial.print("\t");
-      Serial.print(rotatedGyro[1]);
+      Serial.print(gyroVector(1));
       Serial.print("\t");
-      Serial.println(rotatedGyro[2]);
+      Serial.println(gyroVector(2));
       Serial.print(x);
       Serial.print("\t");
       Serial.print(y);

From f9cf6c716283402dd02939e21cf31a0b1b6d810a Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 16 May 2025 12:44:23 +0200
Subject: [PATCH 09/64] Add shaking detection

---
 .../PauliGate_detection.ino                   |  8 +-
 src/Qbead.h                                   | 85 ++++++++++++++-----
 2 files changed, 67 insertions(+), 26 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index d9fb613..453ceb3 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -18,8 +18,8 @@ void animationGate(Qbead::Coordinates oldPoint, int steps, int animationLength)
     float newTheta = q*state.getCoordinates().theta() + (1-q)*oldPoint.theta();
     float newPhi = q*state.getCoordinates().phi() + (1-q)*oldPoint.phi();
     Qbead::Coordinates animationPoint = Qbead::Coordinates(newTheta, newPhi);
-    bead.setLed(animationPoint, color(255, 255, 0));
-    bead.readIMU();
+    bead.setLed(animationPoint, color(255, 255, 0), true);
+    bead.readIMU(false);
     bead.showAxis();
     bead.show();
     delay(animationLength/steps);
@@ -47,7 +47,7 @@ void setup() {
 }
 
 void loop() {
-  bead.readIMU();
+  bead.readIMU(false);
   bead.clear();
   bead.showAxis();
   if (freeze)
@@ -65,7 +65,7 @@ void loop() {
   {
     Qbead::QuantumState oldState = state;
     cooldownColor = color(255, 255, 255);
-    freeze = bead.checkRotation(state);
+    freeze = bead.checkMotion(state);
     timeIMU = micros();
     if (freeze && toggleAnimationOn)
     {
diff --git a/src/Qbead.h b/src/Qbead.h
index 0bc2d24..2f25693 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -22,7 +22,9 @@ using namespace Eigen;
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
-#define GYRO_THRESHOLD 0.0001
+#define GYRO_MEASUREMENT_THRESHOLD 0.0001
+#define GYRO_GATE_THRESHOLD 500
+#define SHAKING_THRESHOLD 0.09
 #define QB_PIXEL_COUNT 62
 
 #define QB_MAX_PRPH_CONNECTION 2
@@ -252,13 +254,12 @@ class QuantumState
     return stateCoordinates;
   }
 
-  int collapse()
+  void collapse()
   {
     const float theta = stateCoordinates.theta();
     const float a = cos(theta / 2);
     const bool is1 = random(0, 100) < a * a * 100;
     this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
-    return is1 ? 1 : 0;
   }
 
   // Rotate PI around the x axis
@@ -547,26 +548,61 @@ class Qbead {
     setBloch_deg(theta, phi, c, true);
   }
 
-  bool checkRotation(QuantumState &toBeRotated)
+  Vector3d getGravity()
   {
-    if (abs(gyroVector[0]) > 300)
+    // substract gravity from the gravity vector
+    Vector3d g = gravity;
+    g.normalize();
+    return gravity - g;
+  }
+
+  bool checkMotion(QuantumState &toBeRotated)
+  {
+    Vector3d gravity = getGravity();
+    u_int8_t n = 0;
+    // store the function in a variable (default is do nothing)
+    void (QuantumState::*func)();
+    if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing X gate");
-      toBeRotated.gateX();
-      return true;
+      func = &QuantumState::gateX;
+      n++;
     }
-    if (abs(gyroVector[1]) > 300)
+    if (abs(gyroVector[1]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Y gate");
-      toBeRotated.gateY();
-      return true;
+      func = &QuantumState::gateY;
+      n++;
     }
-    if (abs(gyroVector[2]) > 300)
+    if (abs(gyroVector[2]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Z gate");
-      toBeRotated.gateZ();
+      func = &QuantumState::gateZ;
+      n++;
+    }
+    if (gravity(2) * gravity(2) > SHAKING_THRESHOLD)
+    {
+      Serial.println("Collapsing");
+      toBeRotated.collapse();
+      func = &QuantumState::collapse;
+      n++;
+    }
+    if (gravity(0) * gravity(0) + gravity(1) * gravity(1) > SHAKING_THRESHOLD)
+    {
+      Serial.println("Hadamard gate");
+      func = &QuantumState::gateH;
+      n++;
+    }
+    if (n == 1)
+    {
+      // execute the function
+      (toBeRotated.*func)();
       return true;
     }
+    else if (n > 1)
+    {
+      Serial.println("Multiple gates detected, ignoring");
+    }
     return false;
   }
 
@@ -587,13 +623,6 @@ class Qbead {
     rxGyro = (1-2*sx)*rgyrobuffer[ix];
     ryGyro = (1-2*sy)*rgyrobuffer[iy];
     rzGyro = (1-2*sz)*rgyrobuffer[iz];
-
-    Serial.print("raw gyro: ");
-    Serial.print(rxGyro);
-    Serial.print("\t");
-    Serial.print(ryGyro);
-    Serial.print("\t");
-    Serial.println(rzGyro);
     
     float T_new = micros();
     float delta = T_new - T_imu;
@@ -626,9 +655,9 @@ class Qbead {
     yGyro = ryGyro * delta * PI / (1000000 * 180);
     // The zGyro has an offset of 0.0006 rad/s
     zGyro = rzGyro * delta * PI / (1000000 * 180) - 0.0006;
-    if (xGyro < GYRO_THRESHOLD && xGyro > -GYRO_THRESHOLD) xGyro = 0;
-    if (yGyro < GYRO_THRESHOLD && yGyro > -GYRO_THRESHOLD) yGyro = 0;
-    if (zGyro < GYRO_THRESHOLD && zGyro > -GYRO_THRESHOLD) zGyro = 0;
+    if (xGyro < GYRO_MEASUREMENT_THRESHOLD && xGyro > -GYRO_MEASUREMENT_THRESHOLD) xGyro = 0;
+    if (yGyro < GYRO_MEASUREMENT_THRESHOLD && yGyro > -GYRO_MEASUREMENT_THRESHOLD) yGyro = 0;
+    if (zGyro < GYRO_MEASUREMENT_THRESHOLD && zGyro > -GYRO_MEASUREMENT_THRESHOLD) zGyro = 0;
 
     gravity = Vector3d(x, y, z);
     Vector3d gyro = Vector3d(xGyro, yGyro, zGyro);
@@ -636,6 +665,12 @@ class Qbead {
     yaw = fmod(yaw, 2 * PI);
 
     if (print) {
+      Serial.print("raw gyro: ");
+      Serial.print(rxGyro);
+      Serial.print("\t");
+      Serial.print(ryGyro);
+      Serial.print("\t");
+      Serial.println(rzGyro);
       Serial.print("gyro: ");
       Serial.print(gyroVector(0));
       Serial.print("\t");
@@ -643,10 +678,16 @@ class Qbead {
       Serial.print("\t");
       Serial.println(gyroVector(2));
       Serial.print(x);
+      Serial.print(" - ");
+      Serial.print(imu.readFloatAccelX());
       Serial.print("\t");
       Serial.print(y);
+      Serial.print(" - ");
+      Serial.print(imu.readFloatAccelY());
       Serial.print("\t");
       Serial.print(z);
+      Serial.print(" - ");
+      Serial.print(imu.readFloatAccelZ());
       Serial.print("\t-1\t1\t");
       Serial.print(xGyro * 1000);
       Serial.print("\t");

From 50a1743349faff20c9850fb2f7de7cddeb39260d Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 16 May 2025 14:57:51 +0200
Subject: [PATCH 10/64] implemented gates using matrices from Eigen

---
 src/Qbead.h | 72 ++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 52 insertions(+), 20 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 4a71049..6d91aea 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -195,6 +195,13 @@ Vector3d v;
     return atan2(y(), x());
   }
 
+  Vector2cf stateVector2D()
+  {
+    std::complex<float> alpha = cos(theta()/2);
+    std::complex<float> beta = exp(std::complex<float>(0, phi())) * sin(theta()/2);
+    return {alpha, beta};
+  }
+
   float dist(Vector3d other) const
   {
     Vector3d diff = v - other;
@@ -276,28 +283,54 @@ class QuantumState
     return is1 ? 1 : 0;
   }
 
+  void applyGate(Matrix2cf gate, float rotationFraction = 1)
+  {
+    // TODO find way to do part of the rotation for animation
+    // gate = gate.pow(rotationFraction); // (Arduino does not support .pow on a matrix)
+    Vector2cf stateVector = stateCoordinates.stateVector2D();
+    stateVector = gate * stateVector;
+    stateVector.normalize();
+    stateCoordinates.set(2*acos(abs(stateVector.x())), arg(stateVector.y()) - arg(stateVector.x()));
+  }
+
   // Rotate PI around the x axis
-  void gateX()
+  void gateX(float rotationFraction = 1)
   {
-    stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
+    Matrix2cf gateMatrix;
+    gateMatrix << std::complex<float>(0, 0), std::complex<float>(1, 0), 
+                  std::complex<float>(1, 0), std::complex<float>(0, 0);
+    applyGate(gateMatrix, rotationFraction);
+    //stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the y axis
-  void gateZ()
-  {
-    stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
+  void gateZ(float rotationFraction = 1)
+  { 
+    Matrix2cf gateMatrix;
+    gateMatrix << std::complex<float>(1, 0), std::complex<float>(0, 0), 
+                  std::complex<float>(0, 0), std::complex<float>(-1, 0);
+    applyGate(gateMatrix, rotationFraction);
+    //stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
   }
 
   // Rotate PI around the z axis
-  void gateY()
+  void gateY(float rotationFraction = 1)
   {
-    stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
+    Matrix2cf gateMatrix;
+    gateMatrix << std::complex<float>(0, 0), std::complex<float>(0, -1),
+                  std::complex<float>(0, 1), std::complex<float>(0, 0);
+    applyGate(gateMatrix, rotationFraction);
+    //stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the xz axis
-  void gateH()
+  void gateH(float rotationFraction = 1)
   {
-    stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
+    Matrix2cf gateMatrix;
+    gateMatrix << std::complex<float>(1/sqrt(2), 0), std::complex<float>(1/sqrt(2), 0), 
+                  std::complex<float>(1/sqrt(2), 0), std::complex<float>(-1/sqrt(2), 0);
+    applyGate(gateMatrix, rotationFraction);
+    //stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
   }
 };
 
@@ -506,7 +539,7 @@ class Qbead {
   }
 
   void setLed(Coordinates coordinates, uint32_t color, bool smooth = false) {
-    Coordinates adjusted = getRelativeCoordinates(coordinates);
+    Coordinates adjusted = coordinates;   // getRelativeCoordinates(coordinates);
     float theta = adjusted.theta() * 180 / PI;
     float phi = adjusted.phi() * 180 / PI;
     if (phi < 0) {
@@ -583,7 +616,7 @@ class Qbead {
 
   bool checkRotation(QuantumState &toBeRotated)
   {
-    if (abs(rotatedGyro[0]) > 300)
+    if (abs(filteredGyro[0]) > 400)
     {
       if (Serial)
       {
@@ -592,7 +625,7 @@ class Qbead {
       toBeRotated.gateX();
       return true;
     }
-    if (abs(rotatedGyro[1]) > 300)
+    if (abs(filteredGyro[1]) > 400)
     {
       if (Serial)
       {
@@ -601,7 +634,7 @@ class Qbead {
       toBeRotated.gateY();
       return true;
     }
-    if (abs(rotatedGyro[2]) > 300)
+    if (abs(filteredGyro[2]) > 300)
     {
       if (Serial)
       {
@@ -630,13 +663,6 @@ class Qbead {
     rxGyro = (1-2*sx)*rgyrobuffer[ix];
     ryGyro = (1-2*sy)*rgyrobuffer[iy];
     rzGyro = (1-2*sz)*rgyrobuffer[iz];
-
-    Serial.print("raw gyro: ");
-    Serial.print(rxGyro);
-    Serial.print("\t");
-    Serial.print(ryGyro);
-    Serial.print("\t");
-    Serial.println(rzGyro);
     
     float T_new = micros();
     float delta = T_new - T_imu;
@@ -685,6 +711,12 @@ class Qbead {
 
 
     if (print) {
+      Serial.print("raw gyro: ");
+      Serial.print(rxGyro);
+      Serial.print("\t");
+      Serial.print(ryGyro);
+      Serial.print("\t");
+      Serial.println(rzGyro);
       Serial.print("rotatedGyro: ");
       Serial.print(rotatedGyro[0]);
       Serial.print("\t");

From 2f29fec4e7e3a1162181e34dee7d8347cf4f70e4 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 16 May 2025 15:37:08 +0200
Subject: [PATCH 11/64] Use tap detection for collapsing

---
 src/Qbead.h | 75 ++++++++++++++++++++++++++++-------------------------
 1 file changed, 40 insertions(+), 35 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index f0941a2..8c066b2 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -1,13 +1,11 @@
 #ifndef QBEAD_H
 #define QBEAD_H
 
-
 #include <Arduino.h>
 #include <Adafruit_NeoPixel.h>
 #include <LSM6DS3.h>
 #include <math.h>
 #include <ArduinoEigen.h>
-
 #include <bluefruit.h>
 
 using namespace Eigen;
@@ -23,7 +21,7 @@ using namespace Eigen;
 #define QB_SY 0
 #define QB_SZ 1
 #define GYRO_MEASUREMENT_THRESHOLD 0.0001
-#define GYRO_GATE_THRESHOLD 500
+#define GYRO_GATE_THRESHOLD 400
 #define SHAKING_THRESHOLD 0.09
 #define QB_PIXEL_COUNT 62
 
@@ -143,6 +141,29 @@ void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
   z = cos(theta);
 }
 
+// Tap detection
+LSM6DS3 myIMU(I2C_MODE, 0x6A);
+uint8_t interruptCount = 0; // Amount of received interrupts
+uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
+
+void setupTapInterrupt() {
+  uint8_t error = 0;
+  uint8_t dataToWrite = 0;
+
+  // Double Tap Config
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x8C);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
+}
+
+void int1ISR()
+{
+  interruptCount++;
+}
+
 namespace Qbead {
 
 class Coordinates
@@ -503,6 +524,11 @@ class Qbead {
     blechargyr.begin();
     blechargyr.write(zerobuffer20, 3*sizeof(float));
     startBLEadv();
+
+    // Tap detection
+    setupTapInterrupt();
+    pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
+    attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
   }
 
   void clear() {
@@ -591,51 +617,30 @@ class Qbead {
 
   bool checkMotion(QuantumState &toBeRotated)
   {
-    Vector3d gravity = getGravity();
-    u_int8_t n = 0;
-    // store the function in a variable (default is do nothing)
-    void (QuantumState::*func)();
+    if (interruptCount > prevInterruptCount) {
+      Serial.println("Interrupt received!");
+      toBeRotated.collapse();
+      prevInterruptCount = interruptCount;
+      return true;
+    }
     if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing X gate");
-      func = &QuantumState::gateX;
-      n++;
+      toBeRotated.gateX();
+      return true;
     }
     if (abs(gyroVector[1]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Y gate");
-      func = &QuantumState::gateY;
-      n++;
+      toBeRotated.gateY();
+      return true;
     }
     if (abs(gyroVector[2]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Z gate");
-      func = &QuantumState::gateZ;
-      n++;
-    }
-    if (gravity(2) * gravity(2) > SHAKING_THRESHOLD)
-    {
-      Serial.println("Collapsing");
-      toBeRotated.collapse();
-      func = &QuantumState::collapse;
-      n++;
-    }
-    if (gravity(0) * gravity(0) + gravity(1) * gravity(1) > SHAKING_THRESHOLD)
-    {
-      Serial.println("Hadamard gate");
-      func = &QuantumState::gateH;
-      n++;
-    }
-    if (n == 1)
-    {
-      // execute the function
-      (toBeRotated.*func)();
+      toBeRotated.gateZ();
       return true;
     }
-    else if (n > 1)
-    {
-      Serial.println("Multiple gates detected, ignoring");
-    }
     return false;
   }
 

From c9535795ebb70552dd4ee97a95d2d6e9ef985e8d Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 10:30:59 +0200
Subject: [PATCH 12/64] Make accelerometer optional

---
 examples/IMU_reader/IMU_reader.ino |  3 ++-
 src/Qbead.h                        | 35 +++++++++++++++++++++---------
 2 files changed, 27 insertions(+), 11 deletions(-)

diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino
index 72ad22c..6fb121b 100644
--- a/examples/IMU_reader/IMU_reader.ino
+++ b/examples/IMU_reader/IMU_reader.ino
@@ -4,6 +4,7 @@ Qbead::Qbead bead;
 
 void setup() {
   bead.begin();
+  bead.startAccelerometer();
   bead.setBrightness(25); // way too bright
   Serial.println("testing all pixels discretely");
   for (int i = 0; i < bead.pixels.numPixels(); i++) {
@@ -25,6 +26,6 @@ void setup() {
 void loop() {
   bead.readIMU();
   bead.clear();
-  bead.setBloch_deg_smooth(bead.t_acc, bead.p_acc, color(255, 0, 255));
+  bead.setLed(Qbead::Coordinates(bead.gravity), color(255, 0, 255), true);
   bead.show();
 }
\ No newline at end of file
diff --git a/src/Qbead.h b/src/Qbead.h
index 8c066b2..b297aba 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -472,6 +472,16 @@ class Qbead {
     Serial.println(singletoninstance->p_ble);
   }
 
+  void startAccelerometer() {
+    // BLE Characteristic IMU xyz accelerometer readout
+    blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
+    blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blecharacc.setUserDescriptor("xyz acceleration");
+    blecharacc.setFixedLen(3*sizeof(float));
+    blecharacc.begin();
+    blecharacc.write(zerobuffer20, 3*sizeof(float));    
+  }
+
   void begin() {
     singletoninstance = this;
     Serial.begin(9600);
@@ -509,13 +519,6 @@ class Qbead {
     blecharsph.setWriteCallback(ble_callback_theta_phi);
     blecharsph.begin();
     blecharsph.write(zerobuffer20, 2);
-    // BLE Characteristic IMU xyz accelerometer readout
-    blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
-    blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN);
-    blecharacc.setUserDescriptor("xyz acceleration");
-    blecharacc.setFixedLen(3*sizeof(float));
-    blecharacc.begin();
-    blecharacc.write(zerobuffer20, 3*sizeof(float));    
     // BLE Characteristic IMU xyz gyroscope readout
     blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
     blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN);
@@ -617,9 +620,21 @@ class Qbead {
 
   bool checkMotion(QuantumState &toBeRotated)
   {
-    if (interruptCount > prevInterruptCount) {
-      Serial.println("Interrupt received!");
-      toBeRotated.collapse();
+    if (interruptCount > prevInterruptCount)
+    {
+      uint8_t tapStatus = 0;
+      myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
+
+      if (tapStatus & 0x05)
+      {
+        Serial.println("Executing H gate");
+        toBeRotated.gateH();
+      }
+      else
+      {
+        Serial.println("Collapsing");
+        toBeRotated.collapse();
+      }
       prevInterruptCount = interruptCount;
       return true;
     }

From e74fb7d3c0b497f7f2a8bb5645918a7c68735c24 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 12:08:40 +0200
Subject: [PATCH 13/64] Switch axis

---
 src/Qbead.h | 155 ++++++++++++++++++++++++++--------------------------
 1 file changed, 78 insertions(+), 77 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index b297aba..baf7352 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -14,9 +14,9 @@ using namespace Eigen;
 #define QB_LEDPIN 10
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
 #define QB_IMU_ADDR 0x6A
-#define QB_IX 0
-#define QB_IY 2
-#define QB_IZ 1
+#define QB_IX 1
+#define QB_IY 0
+#define QB_IZ 2
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
@@ -41,8 +41,9 @@ const uint8_t QB_UUID_GYR_CHAR[] =
 const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 
 // TODO manage namespaces better
+// The setPixelColor switches blue and green
 static uint32_t color(uint8_t r, uint8_t g, uint8_t b) {
-  return ((uint32_t)r << 16) | ((uint32_t)g << 8) | b;
+  return ((uint32_t)r << 16) | ((uint16_t)b << 8) | g;
 }
 
 static uint8_t redch(uint32_t rgb) {
@@ -153,7 +154,7 @@ void setupTapInterrupt() {
   // Double Tap Config
   myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
   myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x8C);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C);
   myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
   myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
   myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
@@ -391,68 +392,68 @@ class Qbead {
   // led map index to Coordinates
   // This map is for the first version of the pcb
   Coordinates led_map_v1[62] = {
-    Coordinates(PI, 0),
-    Coordinates(5 * PI / 6, 9 * PI / 6),
-    Coordinates(4 * PI / 6, 9 * PI / 6),
-    Coordinates(3 * PI / 6, 9 * PI / 6),
-    Coordinates(2 * PI / 6, 9 * PI / 6),
-    Coordinates(PI / 6, 9 * PI / 6),
-    Coordinates(0, 0),
-    Coordinates(5 * PI / 6, 10 * PI / 6),
-    Coordinates(4 * PI / 6, 10 * PI / 6),
-    Coordinates(3 * PI / 6, 10 * PI / 6),
-    Coordinates(2 * PI / 6, 10 * PI / 6),
-    Coordinates(PI / 6, 10 * PI / 6),
-    Coordinates(5 * PI / 6, 11 * PI / 6),
-    Coordinates(4 * PI / 6, 11 * PI / 6),
-    Coordinates(3 * PI / 6, 11 * PI / 6),
-    Coordinates(2 * PI / 6, 11 * PI / 6),
-    Coordinates(PI / 6, 11 * PI / 6),
-    Coordinates(5 * PI / 6, 0),
-    Coordinates(4 * PI / 6, 0),
-    Coordinates(3 * PI / 6, 0),
-    Coordinates(2 * PI / 6, 0),
-    Coordinates(PI / 6, 0),
-    Coordinates(5 * PI / 6, PI / 6),
-    Coordinates(4 * PI / 6, PI / 6),
-    Coordinates(3 * PI / 6, PI / 6),
-    Coordinates(2 * PI / 6, PI / 6),
-    Coordinates(PI / 6, PI / 6),
-    Coordinates(5 * PI / 6, 2 * PI / 6),
-    Coordinates(4 * PI / 6, 2 * PI / 6),
-    Coordinates(3 * PI / 6, 2 * PI / 6),
-    Coordinates(2 * PI / 6, 2 * PI / 6),
-    Coordinates(PI / 6, 2 * PI / 6),
-    Coordinates(5 * PI / 6, 3 * PI / 6),
-    Coordinates(4 * PI / 6, 3 * PI / 6),
-    Coordinates(3 * PI / 6, 3 * PI / 6),
-    Coordinates(2 * PI / 6, 3 * PI / 6),
-    Coordinates(PI / 6, 3 * PI / 6),
-    Coordinates(5 * PI / 6, 4 * PI / 6),
-    Coordinates(4 * PI / 6, 4 * PI / 6),
-    Coordinates(3 * PI / 6, 4 * PI / 6),
-    Coordinates(2 * PI / 6, 4 * PI / 6),
-    Coordinates(PI / 6, 4 * PI / 6),
-    Coordinates(5 * PI / 6, 5 * PI / 6),
-    Coordinates(4 * PI / 6, 5 * PI / 6),
-    Coordinates(3 * PI / 6, 5 * PI / 6),
-    Coordinates(2 * PI / 6, 5 * PI / 6),
-    Coordinates(PI / 6, 5 * PI / 6),
-    Coordinates(5 * PI / 6, 6 * PI / 6),
-    Coordinates(4 * PI / 6, 6 * PI / 6),
-    Coordinates(3 * PI / 6, 6 * PI / 6),
-    Coordinates(2 * PI / 6, 6 * PI / 6),
-    Coordinates(PI / 6, 6 * PI / 6),
-    Coordinates(5 * PI / 6, 7 * PI / 6),
-    Coordinates(4 * PI / 6, 7 * PI / 6),
-    Coordinates(3 * PI / 6, 7 * PI / 6),
-    Coordinates(2 * PI / 6, 7 * PI / 6),
-    Coordinates(PI / 6, 7 * PI / 6),
-    Coordinates(5 * PI / 6, 8 * PI / 6),
-    Coordinates(4 * PI / 6, 8 * PI / 6),
-    Coordinates(3 * PI / 6, 8 * PI / 6),
-    Coordinates(2 * PI / 6, 8 * PI / 6),
-    Coordinates(PI / 6, 8 * PI / 6),
+    Coordinates(-1, -0, -0),
+    Coordinates(-0.866, 0, -0.5),
+    Coordinates(-0.5, 0, -0.866),
+    Coordinates(-0, 0, -1),
+    Coordinates(0.5, 0, -0.866),
+    Coordinates(0.866, 0, -0.5),
+    Coordinates(1, 0, 0),
+    Coordinates(-0.866, 0.25, -0.433),
+    Coordinates(-0.5, 0.433, -0.75),
+    Coordinates(-0, 0.5, -0.866),
+    Coordinates(0.5, 0.433, -0.75),
+    Coordinates(0.866, 0.25, -0.433),
+    Coordinates(-0.866, 0.433, -0.25),
+    Coordinates(-0.5, 0.75, -0.433),
+    Coordinates(-0, 0.866, -0.5),
+    Coordinates(0.5, 0.75, -0.433),
+    Coordinates(0.866, 0.433, -0.25),
+    Coordinates(-0.866, 0.5, 0),
+    Coordinates(-0.5, 0.866, 0),
+    Coordinates(-0, 1, 0),
+    Coordinates(0.5, 0.866, 0),
+    Coordinates(0.866, 0.5, 0),
+    Coordinates(-0.866, 0.433, 0.25),
+    Coordinates(-0.5, 0.75, 0.433),
+    Coordinates(-0, 0.866, 0.5),
+    Coordinates(0.5, 0.75, 0.433),
+    Coordinates(0.866, 0.433, 0.25),
+    Coordinates(-0.866, 0.25, 0.433),
+    Coordinates(-0.5, 0.433, 0.75),
+    Coordinates(-0, 0.5, 0.866),
+    Coordinates(0.5, 0.433, 0.75),
+    Coordinates(0.866, 0.25, 0.433),
+    Coordinates(-0.866, -0, 0.5),
+    Coordinates(-0.5, -0, 0.866),
+    Coordinates(-0, -0, 1),
+    Coordinates(0.5, -0, 0.866),
+    Coordinates(0.866, -0, 0.5),
+    Coordinates(-0.866, -0.25, 0.433),
+    Coordinates(-0.5, -0.433, 0.75),
+    Coordinates(-0, -0.5, 0.866),
+    Coordinates(0.5, -0.433, 0.75),
+    Coordinates(0.866, -0.25, 0.433),
+    Coordinates(-0.866, -0.433, 0.25),
+    Coordinates(-0.5, -0.75, 0.433),
+    Coordinates(-0, -0.866, 0.5),
+    Coordinates(0.5, -0.75, 0.433),
+    Coordinates(0.866, -0.433, 0.25),
+    Coordinates(-0.866, -0.5, -0),
+    Coordinates(-0.5, -0.866, -0),
+    Coordinates(-0, -1, -0),
+    Coordinates(0.5, -0.866, -0),
+    Coordinates(0.866, -0.5, -0),
+    Coordinates(-0.866, -0.433, -0.25),
+    Coordinates(-0.5, -0.75, -0.433),
+    Coordinates(-0, -0.866, -0.5),
+    Coordinates(0.5, -0.75, -0.433),
+    Coordinates(0.866, -0.433, -0.25),
+    Coordinates(-0.866, -0.25, -0.433),
+    Coordinates(-0.5, -0.433, -0.75),
+    Coordinates(-0, -0.5, -0.866),
+    Coordinates(0.5, -0.433, -0.75),
+    Coordinates(0.866, -0.25, -0.433),
   };
 
   static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
@@ -556,12 +557,12 @@ class Qbead {
   }
 
   void showAxis() {
-    setLed(Coordinates(0, 0, 1), color(255, 0, 0));
+    setLed(Coordinates(1, 0, 0), color(0, 0, 122));
+    setLed(Coordinates(-1, 0, 0), color(0, 0, 122));
+    setLed(Coordinates(0, 1, 0), color(0, 0, 122));
+    setLed(Coordinates(0, -1, 0), color(0, 0, 122));
+    setLed(Coordinates(0, 0, 1), color(0, 255, 0));
     setLed(Coordinates(0, 0, -1), color(255, 0, 0));
-    setLed(Coordinates(0, 1, 0), color(0, 255, 0));
-    setLed(Coordinates(0, -1, 0), color(0, 255, 0));
-    setLed(Coordinates(1, 0, 0), color(0, 0, 255));
-    setLed(Coordinates(-1, 0, 0), color(0, 0, 255));
   }
 
   // in rads
@@ -625,15 +626,15 @@ class Qbead {
       uint8_t tapStatus = 0;
       myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
 
-      if (tapStatus & 0x05)
+      if (tapStatus & 0x01)
       {
-        Serial.println("Executing H gate");
-        toBeRotated.gateH();
+        Serial.println("Collapsing");
+        toBeRotated.collapse();
       }
       else
       {
-        Serial.println("Collapsing");
-        toBeRotated.collapse();
+        Serial.println("Executing H gate");
+        toBeRotated.gateH();
       }
       prevInterruptCount = interruptCount;
       return true;

From 035f7cc8ee81da498217f63c44f32c34b4298082 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 12:16:08 +0200
Subject: [PATCH 14/64] Enable tap freezing

---
 examples/PauliGate_detection/PauliGate_detection.ino | 1 +
 1 file changed, 1 insertion(+)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 453ceb3..77b43cb 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -56,6 +56,7 @@ void loop() {
     float newTime = micros();
     counter += newTime - timeIMU;
     timeIMU = newTime;
+    prevInterruptCount = interruptCount;
     if (counter > 3000000)
     {
       counter = 0;

From 31f78df22298af25fb22e2e35fa8c96adfe7af85 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Mon, 19 May 2025 15:01:13 +0200
Subject: [PATCH 15/64] added partial rotation and animations using matrices

---
 .../PauliGate_detection.ino                   | 69 ++++++++++---------
 src/Qbead.h                                   | 33 +++++----
 2 files changed, 51 insertions(+), 51 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 77b43cb..86fbac0 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -1,26 +1,44 @@
 #include <Qbead.h>
 
 Qbead::Qbead bead;
-Qbead::QuantumState state = Qbead::QuantumState(Qbead::Coordinates(1, 2.3));
-bool freeze = 0;
-float timeIMU = 0;
-float counter = 0;
-uint32_t cooldownColor = color(255, 255, 255);
+Qbead::QuantumState state = Qbead::QuantumState(Qbead::Coordinates(1, 0));
+int rotationState;
+uint32_t stateColor = color(255, 255, 255);
 const bool toggleAnimationOn = 1;
 
 
-void animationGate(Qbead::Coordinates oldPoint, int steps, int animationLength)
+void animationGate(int gateType, int steps, int animationLength)
 {
+  if (gateType == 5)
+  {
+    state.collapse();
+    return;
+  }
+  if (!toggleAnimationOn)
+  {
+    steps = 1;
+  }
+  float stepLength = M_PI / (float) steps;
+  stateColor = color(255, 0, 255);
   for (int i = 0; i < steps; i++)
   {
     bead.clear();
-    float q = i/ (float) steps;
-    float newTheta = q*state.getCoordinates().theta() + (1-q)*oldPoint.theta();
-    float newPhi = q*state.getCoordinates().phi() + (1-q)*oldPoint.phi();
-    Qbead::Coordinates animationPoint = Qbead::Coordinates(newTheta, newPhi);
-    bead.setLed(animationPoint, color(255, 255, 0), true);
-    bead.readIMU(false);
     bead.showAxis();
+    switch (gateType)
+    {
+      case 1:
+        state.gateX(stepLength);
+        break;
+      case 2:
+        state.gateY(stepLength);
+        break;
+      case 3:
+        state.gateZ(stepLength);
+        break;
+      case 4:
+        state.gateH(stepLength);
+    }
+    bead.setLed(state.getCoordinates(), stateColor);
     bead.show();
     delay(animationLength/steps);
   }
@@ -50,29 +68,12 @@ void loop() {
   bead.readIMU(false);
   bead.clear();
   bead.showAxis();
-  if (freeze)
+  stateColor = color(255, 255, 255);
+  rotationState = bead.checkMotion();
+  if (rotationState != 0) 
   {
-    cooldownColor = color(255, 255, 0);
-    float newTime = micros();
-    counter += newTime - timeIMU;
-    timeIMU = newTime;
-    prevInterruptCount = interruptCount;
-    if (counter > 3000000)
-    {
-      counter = 0;
-      freeze = 0;
-    }
-  } else
-  {
-    Qbead::QuantumState oldState = state;
-    cooldownColor = color(255, 255, 255);
-    freeze = bead.checkMotion(state);
-    timeIMU = micros();
-    if (freeze && toggleAnimationOn)
-    {
-      animationGate(oldState.getCoordinates(), 30, 4000);
-    }
+    animationGate(rotationState, 10, 4000);
   }
-  bead.setLed(state.getCoordinates(), cooldownColor);
+  bead.setLed(state.getCoordinates(), stateColor);
   bead.show();
 }
\ No newline at end of file
diff --git a/src/Qbead.h b/src/Qbead.h
index baf7352..3948e6a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -39,6 +39,7 @@ const uint8_t QB_UUID_GYR_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
 
 const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+const std::complex<float>i(0, 1);
 
 // TODO manage namespaces better
 // The setPixelColor switches blue and green
@@ -222,7 +223,7 @@ class Coordinates
   Vector2cf stateVector2D()
   {
     std::complex<float> alpha = cos(theta()/2);
-    std::complex<float> beta = exp(std::complex<float>(0, phi())) * sin(theta()/2);
+    std::complex<float> beta = exp(i*phi()) * sin(theta()/2);
     return {alpha, beta};
   }
 
@@ -291,10 +292,8 @@ class QuantumState
     this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
   }
 
-  void applyGate(Matrix2cf gate, float rotationFraction = 1)
+  void applyGate(Matrix2cf gate)
   {
-    // TODO find way to do part of the rotation for animation
-    // gate = gate.pow(rotationFraction); // (Arduino does not support .pow on a matrix)
     Vector2cf stateVector = stateCoordinates.stateVector2D();
     stateVector = gate * stateVector;
     stateVector.normalize();
@@ -302,32 +301,32 @@ class QuantumState
   }
 
   // Rotate PI around the x axis
-  void gateX(float rotationFraction = 1)
+  void gateX(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << std::complex<float>(0, 0), std::complex<float>(1, 0), 
-                  std::complex<float>(1, 0), std::complex<float>(0, 0);
-    applyGate(gateMatrix, rotationFraction);
+    gateMatrix << cos(rotationDegree/2.0f), -sin(rotationDegree/2.0f)*i, 
+                  -sin(rotationDegree/2.0f)*i, cos(rotationDegree/2.0f);
+    applyGate(gateMatrix);
     //stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the y axis
-  void gateZ(float rotationFraction = 1)
+  void gateZ(float rotationFraction = PI)
   { 
     Matrix2cf gateMatrix;
-    gateMatrix << std::complex<float>(1, 0), std::complex<float>(0, 0), 
-                  std::complex<float>(0, 0), std::complex<float>(-1, 0);
-    applyGate(gateMatrix, rotationFraction);
+    gateMatrix << exp(-i*rotationFraction/2.0f), 0, 
+                  0, exp(i*rotationFraction/2.0f);
+    applyGate(gateMatrix);
     //stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
   }
 
   // Rotate PI around the z axis
-  void gateY(float rotationFraction = 1)
+  void gateY(float rotationFraction = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << std::complex<float>(0, 0), std::complex<float>(0, -1),
-                  std::complex<float>(0, 1), std::complex<float>(0, 0);
-    applyGate(gateMatrix, rotationFraction);
+    gateMatrix << cos(rotationFraction/2.0f), -sin(rotationFraction/2.0f),
+                  sin(rotationFraction/2.0f), cos(rotationFraction/2.0f);
+    applyGate(gateMatrix);
     //stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
   }
 
@@ -337,7 +336,7 @@ class QuantumState
     Matrix2cf gateMatrix;
     gateMatrix << std::complex<float>(1/sqrt(2), 0), std::complex<float>(1/sqrt(2), 0), 
                   std::complex<float>(1/sqrt(2), 0), std::complex<float>(-1/sqrt(2), 0);
-    applyGate(gateMatrix, rotationFraction);
+    applyGate(gateMatrix);
     //stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
   }
 };

From 65cd4143c166c051780ef7b3b91e7c33c6bfb29f Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Mon, 19 May 2025 15:08:08 +0200
Subject: [PATCH 16/64] added last changes from partial rotations

---
 src/Qbead.h | 38 +++++++++++++++++---------------------
 1 file changed, 17 insertions(+), 21 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 3948e6a..d8f1786 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -305,37 +305,37 @@ class QuantumState
   {
     Matrix2cf gateMatrix;
     gateMatrix << cos(rotationDegree/2.0f), -sin(rotationDegree/2.0f)*i, 
-                  -sin(rotationDegree/2.0f)*i, cos(rotationDegree/2.0f);
+                  -sin(rotationDegree/2.0f)*i, cos(rotationDegree/2.0f); // gloabal phase differs from pauli gates but thid doesn't matter for bloch sphere
     applyGate(gateMatrix);
     //stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the y axis
-  void gateZ(float rotationFraction = PI)
+  void gateZ(float rotationDegree = PI)
   { 
     Matrix2cf gateMatrix;
-    gateMatrix << exp(-i*rotationFraction/2.0f), 0, 
-                  0, exp(i*rotationFraction/2.0f);
+    gateMatrix << exp(-i*rotationDegree/2.0f), 0, 
+                  0, exp(i*rotationDegree/2.0f);
     applyGate(gateMatrix);
     //stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
   }
 
   // Rotate PI around the z axis
-  void gateY(float rotationFraction = PI)
+  void gateY(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << cos(rotationFraction/2.0f), -sin(rotationFraction/2.0f),
-                  sin(rotationFraction/2.0f), cos(rotationFraction/2.0f);
+    gateMatrix << cos(rotationDegree/2.0f), -sin(rotationDegree/2.0f),
+                  sin(rotationDegree/2.0f), cos(rotationDegree/2.0f);
     applyGate(gateMatrix);
     //stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the xz axis
-  void gateH(float rotationFraction = 1)
+  void gateH(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << std::complex<float>(1/sqrt(2), 0), std::complex<float>(1/sqrt(2), 0), 
-                  std::complex<float>(1/sqrt(2), 0), std::complex<float>(-1/sqrt(2), 0);
+    gateMatrix << (cos(rotationDegree/2.0f) - i*sin(rotationDegree/2.0f)/sqrt(2.0f)), -i*sin(rotationDegree/2.0f)/sqrt(2.0f), 
+                  -i*sin(rotationDegree/2.0f)/sqrt(2.0f), (cos(rotationDegree/2.0f) + i*sin(rotationDegree/2.0f)/sqrt(2.0f));
     applyGate(gateMatrix);
     //stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
   }
@@ -618,7 +618,7 @@ class Qbead {
     return gravity - g;
   }
 
-  bool checkMotion(QuantumState &toBeRotated)
+  int checkMotion()
   {
     if (interruptCount > prevInterruptCount)
     {
@@ -628,35 +628,31 @@ class Qbead {
       if (tapStatus & 0x01)
       {
         Serial.println("Collapsing");
-        toBeRotated.collapse();
+        return 5;
       }
       else
       {
         Serial.println("Executing H gate");
-        toBeRotated.gateH();
       }
       prevInterruptCount = interruptCount;
-      return true;
+      return 4;
     }
     if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing X gate");
-      toBeRotated.gateX();
-      return true;
+      return 1;
     }
     if (abs(gyroVector[1]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Y gate");
-      toBeRotated.gateY();
-      return true;
+      return 2;
     }
     if (abs(gyroVector[2]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Z gate");
-      toBeRotated.gateZ();
-      return true;
+      return 3;
     }
-    return false;
+    return 0;
   }
 
   void readIMU(bool print=true) {

From 7f647f1fa6c8c54f6859677f2d85fa8e39f4f626 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 15:15:34 +0200
Subject: [PATCH 17/64] Make code more clean

---
 examples/IMU_reader/IMU_reader.ino            |   2 +-
 .../PauliGate_detection.ino                   |   2 +-
 src/Qbead.h                                   | 148 +++++-------------
 3 files changed, 45 insertions(+), 107 deletions(-)

diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino
index 6fb121b..9cac340 100644
--- a/examples/IMU_reader/IMU_reader.ino
+++ b/examples/IMU_reader/IMU_reader.ino
@@ -26,6 +26,6 @@ void setup() {
 void loop() {
   bead.readIMU();
   bead.clear();
-  bead.setLed(Qbead::Coordinates(bead.gravity), color(255, 0, 255), true);
+  bead.setLed(Qbead::Coordinates(bead.gravityVector), color(255, 0, 255), true);
   bead.show();
 }
\ No newline at end of file
diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 86fbac0..91b4b2c 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -65,7 +65,7 @@ void setup() {
 }
 
 void loop() {
-  bead.readIMU(false);
+  bead.readIMU(true);
   bead.clear();
   bead.showAxis();
   stateColor = color(255, 255, 255);
diff --git a/src/Qbead.h b/src/Qbead.h
index d8f1786..b49bfe0 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -21,11 +21,12 @@ using namespace Eigen;
 #define QB_SY 0
 #define QB_SZ 1
 #define GYRO_MEASUREMENT_THRESHOLD 0.0001
-#define GYRO_GATE_THRESHOLD 400
+#define GYRO_GATE_THRESHOLD 7
 #define SHAKING_THRESHOLD 0.09
 #define QB_PIXEL_COUNT 62
-
 #define QB_MAX_PRPH_CONNECTION 2
+#define T_ACC 100000
+#define T_GYRO 10000
 
 const uint8_t QB_UUID_SERVICE[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
@@ -378,10 +379,8 @@ class Qbead {
   const uint8_t ix, iy, iz;
   const bool sx, sy, sz;
   float rbuffer[3], rgyrobuffer[3];
-  float x, y, z, rx, ry, rz; // filtered and raw acc, in units of g
-  float xGyro, yGyro, zGyro, rxGyro, ryGyro, rzGyro; // filtered and raw gyro measurements, in units of deg/s
   float T_imu;             // last update from the IMU
-  Vector3d gravity = Vector3d(0, 0, 1); // gravity vector
+  Vector3d gravityVector = Vector3d(0, 0, 1); // gravity vector
   Vector3d gyroVector = Vector3d(0, 0, 1); // gyro vector
   float yaw;
 
@@ -610,20 +609,14 @@ class Qbead {
     setBloch_deg(theta, phi, c, true);
   }
 
-  Vector3d getGravity()
-  {
-    // substract gravity from the gravity vector
-    Vector3d g = gravity;
-    g.normalize();
-    return gravity - g;
-  }
-
   int checkMotion()
   {
+    // Handle tap interrupt
     if (interruptCount > prevInterruptCount)
     {
       uint8_t tapStatus = 0;
       myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
+      prevInterruptCount = interruptCount;
 
       if (tapStatus & 0x01)
       {
@@ -633,10 +626,10 @@ class Qbead {
       else
       {
         Serial.println("Executing H gate");
+        return 4;
       }
-      prevInterruptCount = interruptCount;
-      return 4;
     }
+    // Handle shaking
     if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing X gate");
@@ -655,6 +648,26 @@ class Qbead {
     return 0;
   }
 
+  void writeToBLE(BLECharacteristic& destination, Vector3d vector) {
+    float buffer[3] = {vector(0), vector(1), vector(2)};
+    destination.write(buffer, 3 * sizeof(float));
+    for (uint16_t conn_hdl = 0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
+    {
+      if (Bluefruit.connected(conn_hdl) && destination.notifyEnabled(conn_hdl))
+      {
+        destination.notify(buffer, 3 * sizeof(float));
+      }
+    }
+  }
+
+  Vector3d getVectorFromBuffer(float *buffer) {
+    // calibration of imu because imu is not aligned with bloch sphere
+    float rx = (1 - 2 * sx) * buffer[ix];
+    float ry = (1 - 2 * sy) * buffer[iy];
+    float rz = (1 - 2 * sz) * buffer[iz];
+    return Vector3d(rx, ry, rz);
+  }
+
   void readIMU(bool print=true) {
     rbuffer[0] = imu.readFloatAccelX();
     rbuffer[1] = imu.readFloatAccelY();
@@ -663,112 +676,37 @@ class Qbead {
     rgyrobuffer[1] = imu.readFloatGyroY();
     rgyrobuffer[2] = imu.readFloatGyroZ();
 
-    // calibration of imu because imu is not aligned with bloch sphere
-    rx = (1-2*sx)*rbuffer[ix];
-    ry = (1-2*sy)*rbuffer[iy];
-    rz = (1-2*sz)*rbuffer[iz];
-
-    // calibration of imu because imu is not aligned with bloch sphere
-    rxGyro = (1-2*sx)*rgyrobuffer[ix];
-    ryGyro = (1-2*sy)*rgyrobuffer[iy];
-    rzGyro = (1-2*sz)*rgyrobuffer[iz];
-    
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
-    // timefilter filter
-    const float T_acc = 100000; // 100 ms // TODO make the filter timeconstant configurable
-    const float T_gyr = 10000; // 10 ms
-    if (delta > 10000)
-    {
-      gyroVector = Vector3d(rxGyro, ryGyro, rzGyro);
-    }
-    else
-    {
-      float d = delta / T_gyr;
-      gyroVector = d * Vector3d(rxGyro, ryGyro, rzGyro) + (1 - d) * gyroVector;
-    }
-    if (delta > 100000) {
-      x = rx;
-      y = ry;
-      z = rz;
-    } else {
-      float d = delta/T_acc;
-      x = d*rx+(1-d)*x;
-      y = d*ry+(1-d)*y;
-      z = d*rz+(1-d)*z;
-    }
 
-    // Gyroscope
-    xGyro = rxGyro * delta * PI / (1000000 * 180);
-    yGyro = ryGyro * delta * PI / (1000000 * 180);
-    // The zGyro has an offset of 0.0006 rad/s
-    zGyro = rzGyro * delta * PI / (1000000 * 180) - 0.0006;
-    if (xGyro < GYRO_MEASUREMENT_THRESHOLD && xGyro > -GYRO_MEASUREMENT_THRESHOLD) xGyro = 0;
-    if (yGyro < GYRO_MEASUREMENT_THRESHOLD && yGyro > -GYRO_MEASUREMENT_THRESHOLD) yGyro = 0;
-    if (zGyro < GYRO_MEASUREMENT_THRESHOLD && zGyro > -GYRO_MEASUREMENT_THRESHOLD) zGyro = 0;
-
-    gravity = Vector3d(x, y, z);
-    Vector3d gyro = Vector3d(xGyro, yGyro, zGyro);
-    yaw += gravity.dot(gyro);
+    Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
+    float d = min(delta / float(T_GYRO), 1.0f);
+    gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
+
+    Vector3d newGravity = getVectorFromBuffer(rbuffer);
+    d = min(delta / float(T_ACC), 1.0f);
+    gravityVector = d * newGravity + (1 - d) * gravityVector;
+
+    yaw += gravityVector.dot(gyroVector);
     yaw = fmod(yaw, 2 * PI);
 
     if (print) {
-      Serial.print("raw gyro: ");
-      Serial.print(rxGyro);
+      Serial.print(gravityVector(0));
       Serial.print("\t");
-      Serial.print(ryGyro);
+      Serial.print(gravityVector(1));
       Serial.print("\t");
-      Serial.println(rzGyro);
-      Serial.print("gyro: ");
+      Serial.print(gravityVector(2));
+      Serial.print("\t-1\t1\t");
       Serial.print(gyroVector(0));
       Serial.print("\t");
       Serial.print(gyroVector(1));
       Serial.print("\t");
       Serial.println(gyroVector(2));
-      Serial.print(x);
-      Serial.print(" - ");
-      Serial.print(imu.readFloatAccelX());
-      Serial.print("\t");
-      Serial.print(y);
-      Serial.print(" - ");
-      Serial.print(imu.readFloatAccelY());
-      Serial.print("\t");
-      Serial.print(z);
-      Serial.print(" - ");
-      Serial.print(imu.readFloatAccelZ());
-      Serial.print("\t-1\t1\t");
-      Serial.print(xGyro * 1000);
-      Serial.print("\t");
-      Serial.print(yGyro * 1000);
-      Serial.print("\t");
-      Serial.print(zGyro * 1000);
-      Serial.print("\t");
-      Serial.println(yaw * 180 / PI);
     }
 
-    rbuffer[0] = x;
-    rbuffer[1] = y;
-    rbuffer[2] = z;
-    blecharacc.write(rbuffer, 3*sizeof(float));
-    for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
-    {
-      if ( Bluefruit.connected(conn_hdl) && blecharacc.notifyEnabled(conn_hdl) )
-      {
-        blecharacc.notify(rbuffer, 3*sizeof(float));
-      }
-    }
-    rgyrobuffer[0] = xGyro;
-    rgyrobuffer[1] = yGyro;
-    rgyrobuffer[2] = zGyro;
-    blechargyr.write(rgyrobuffer, 3*sizeof(float));
-    for (uint16_t conn_hdl=0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
-    {
-      if ( Bluefruit.connected(conn_hdl) && blechargyr.notifyEnabled(conn_hdl) )
-      {
-        blechargyr.notify(rbuffer, 3*sizeof(float));
-      }
-    }
+    writeToBLE(blecharacc, gravityVector);
+    writeToBLE(blechargyr, gyroVector);
   }
 
   void startBLEadv(void)

From 8765628af3f99c20f0f6819d14be7eb339659919 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 15:29:24 +0200
Subject: [PATCH 18/64] Make code more clean

---
 .../PauliGate_detection.ino                   |  3 ++
 src/Qbead.h                                   | 38 +++++--------------
 2 files changed, 13 insertions(+), 28 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 91b4b2c..f7b6ad6 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -27,12 +27,15 @@ void animationGate(int gateType, int steps, int animationLength)
     switch (gateType)
     {
       case 1:
+        Serial.println("Executing X gate");
         state.gateX(stepLength);
         break;
       case 2:
+        Serial.println("Executing Y gate");
         state.gateY(stepLength);
         break;
       case 3:
+        Serial.println("Executing Z gate");
         state.gateZ(stepLength);
         break;
       case 4:
diff --git a/src/Qbead.h b/src/Qbead.h
index b49bfe0..5b375fd 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -346,23 +346,15 @@ class Qbead {
 public:
   Qbead(const uint16_t pin00 = QB_LEDPIN,
         const uint16_t pixelconfig = QB_PIXELCONFIG,
-        const uint8_t imu_addr = QB_IMU_ADDR,
-        const uint8_t ix = QB_IX,
-        const uint8_t iy = QB_IY,
-        const uint8_t iz = QB_IZ,
-        const bool sx = QB_SX,
-        const bool sy = QB_SY,
-        const bool sz = QB_SZ)
+        const uint8_t imu_addr = QB_IMU_ADDR)
       : imu(LSM6DS3(I2C_MODE, imu_addr)),
         pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)),
-        ix(ix), iy(iy), iz(iz),
-        sx(sx), sy(sy), sz(sz),
         bleservice(QB_UUID_SERVICE),
         blecharcol(QB_UUID_COL_CHAR),
         blecharsph(QB_UUID_SPH_CHAR),
         blecharacc(QB_UUID_ACC_CHAR),
         blechargyr(QB_UUID_GYR_CHAR)
-        {}
+  {}
 
   static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
 
@@ -376,8 +368,6 @@ class Qbead {
   BLECharacteristic blechargyr;
   uint8_t connection_count = 0;
 
-  const uint8_t ix, iy, iz;
-  const bool sx, sy, sz;
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   Vector3d gravityVector = Vector3d(0, 0, 1); // gravity vector
@@ -630,20 +620,12 @@ class Qbead {
       }
     }
     // Handle shaking
-    if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
+    for (int i = 0; i < 3; i++)
     {
-      Serial.println("Executing X gate");
-      return 1;
-    }
-    if (abs(gyroVector[1]) > GYRO_GATE_THRESHOLD)
-    {
-      Serial.println("Executing Y gate");
-      return 2;
-    }
-    if (abs(gyroVector[2]) > GYRO_GATE_THRESHOLD)
-    {
-      Serial.println("Executing Z gate");
-      return 3;
+      if (abs(gyroVector[i]) > GYRO_GATE_THRESHOLD)
+      {
+        return i + 1; // 1 = x, 2 = y, 3 = z
+      }
     }
     return 0;
   }
@@ -662,9 +644,9 @@ class Qbead {
 
   Vector3d getVectorFromBuffer(float *buffer) {
     // calibration of imu because imu is not aligned with bloch sphere
-    float rx = (1 - 2 * sx) * buffer[ix];
-    float ry = (1 - 2 * sy) * buffer[iy];
-    float rz = (1 - 2 * sz) * buffer[iz];
+    float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
+    float ry = (1 - 2 * QB_SY) * buffer[QB_IY];
+    float rz = (1 - 2 * QB_SZ) * buffer[QB_IZ];
     return Vector3d(rx, ry, rz);
   }
 

From 248ae6a893a66d2a25ea9f29be0c020e0873eedc Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 19 May 2025 15:55:37 +0200
Subject: [PATCH 19/64] Remove unused code

---
 src/Qbead.h | 41 +++++++++++------------------------------
 1 file changed, 11 insertions(+), 30 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 5b375fd..24bf95a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -194,31 +194,16 @@ class Coordinates
     v.normalize();
   }
 
-  float x()
-  {
-    return v(0);
-  }
-
-  float y()
-  {
-    return v(1);
-  }
-
-  float z()
-  {
-    return v(2);
-  }
-
   // In rads
   float theta()
   {
-    return acos(z());
+    return acos(v(2));
   }
 
   // In rads
   float phi()
   {
-    return atan2(y(), x());
+    return atan2(v(1), v(0));
   }
 
   Vector2cf stateVector2D()
@@ -305,40 +290,36 @@ class QuantumState
   void gateX(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << cos(rotationDegree/2.0f), -sin(rotationDegree/2.0f)*i, 
-                  -sin(rotationDegree/2.0f)*i, cos(rotationDegree/2.0f); // gloabal phase differs from pauli gates but thid doesn't matter for bloch sphere
+    gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f) * i,
+        -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // gloabal phase differs from pauli gates but thid doesn't matter for bloch sphere
     applyGate(gateMatrix);
-    //stateCoordinates.set(stateCoordinates.x(), -stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the y axis
   void gateZ(float rotationDegree = PI)
-  { 
+  {
     Matrix2cf gateMatrix;
-    gateMatrix << exp(-i*rotationDegree/2.0f), 0, 
-                  0, exp(i*rotationDegree/2.0f);
+    gateMatrix << exp(-i * rotationDegree / 2.0f), 0,
+        0, exp(i * rotationDegree / 2.0f);
     applyGate(gateMatrix);
-    //stateCoordinates.set(-stateCoordinates.x(), -stateCoordinates.y(), stateCoordinates.z());
   }
 
   // Rotate PI around the z axis
   void gateY(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << cos(rotationDegree/2.0f), -sin(rotationDegree/2.0f),
-                  sin(rotationDegree/2.0f), cos(rotationDegree/2.0f);
+    gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f),
+        sin(rotationDegree / 2.0f), cos(rotationDegree / 2.0f);
     applyGate(gateMatrix);
-    //stateCoordinates.set(-stateCoordinates.x(), stateCoordinates.y(), -stateCoordinates.z());
   }
 
   // Rotate PI around the xz axis
   void gateH(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
-    gateMatrix << (cos(rotationDegree/2.0f) - i*sin(rotationDegree/2.0f)/sqrt(2.0f)), -i*sin(rotationDegree/2.0f)/sqrt(2.0f), 
-                  -i*sin(rotationDegree/2.0f)/sqrt(2.0f), (cos(rotationDegree/2.0f) + i*sin(rotationDegree/2.0f)/sqrt(2.0f));
+    gateMatrix << (cos(rotationDegree / 2.0f) - i * sin(rotationDegree / 2.0f) / sqrt(2.0f)), -i * sin(rotationDegree / 2.0f) / sqrt(2.0f),
+        -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), (cos(rotationDegree / 2.0f) + i * sin(rotationDegree / 2.0f) / sqrt(2.0f));
     applyGate(gateMatrix);
-    //stateCoordinates.set(stateCoordinates.z(), stateCoordinates.y(), stateCoordinates.x()); //flip x and z axis
   }
 };
 

From ba927d42307ba8f71ba9bc20ca632934e7ae4a78 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 20 May 2025 11:24:01 +0200
Subject: [PATCH 20/64] rotation of gate matches rotation of turn

---
 .../PauliGate_detection.ino                   | 17 ++++++++++---
 src/Qbead.h                                   | 25 +++++++++++++++----
 2 files changed, 33 insertions(+), 9 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 91b4b2c..b71a792 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -9,7 +9,7 @@ const bool toggleAnimationOn = 1;
 
 void animationGate(int gateType, int steps, int animationLength)
 {
-  if (gateType == 5)
+  if (gateType == 8)
   {
     state.collapse();
     return;
@@ -27,15 +27,24 @@ void animationGate(int gateType, int steps, int animationLength)
     switch (gateType)
     {
       case 1:
-        state.gateX(stepLength);
+        state.gateX(-stepLength);
         break;
       case 2:
-        state.gateY(stepLength);
+        state.gateY(-stepLength);
         break;
       case 3:
-        state.gateZ(stepLength);
+        state.gateZ(-stepLength);
         break;
       case 4:
+        state.gateX(stepLength);
+        break;
+      case 5:
+        state.gateY(stepLength);
+        break;
+      case 6:
+        state.gateZ(stepLength);
+        break;
+      case 7:
         state.gateH(stepLength);
     }
     bead.setLed(state.getCoordinates(), stateColor);
diff --git a/src/Qbead.h b/src/Qbead.h
index b49bfe0..58cccdc 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -621,26 +621,41 @@ class Qbead {
       if (tapStatus & 0x01)
       {
         Serial.println("Collapsing");
-        return 5;
+        return 8;
       }
       else
       {
         Serial.println("Executing H gate");
-        return 4;
+        return 7;
       }
     }
     // Handle shaking
-    if (abs(gyroVector[0]) > GYRO_GATE_THRESHOLD)
+    if (gyroVector[0] > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing X gate");
       return 1;
     }
-    if (abs(gyroVector[1]) > GYRO_GATE_THRESHOLD)
+    if (gyroVector[1] > GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Y gate");
       return 2;
     }
-    if (abs(gyroVector[2]) > GYRO_GATE_THRESHOLD)
+    if (gyroVector[2] > GYRO_GATE_THRESHOLD)
+    {
+      Serial.println("Executing Z gate");
+      return 6;
+    }
+    if (gyroVector[0] < -GYRO_GATE_THRESHOLD)
+    {
+      Serial.println("Executing X gate");
+      return 4;
+    }
+    if (gyroVector[1] < -GYRO_GATE_THRESHOLD)
+    {
+      Serial.println("Executing Y gate");
+      return 5;
+    }
+    if (gyroVector[2] < -GYRO_GATE_THRESHOLD)
     {
       Serial.println("Executing Z gate");
       return 3;

From 8b9f9210c2726ed6411d1929a60337009eb75c72 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 20 May 2025 11:25:57 +0200
Subject: [PATCH 21/64] Got rid of warning from unit conversion

---
 src/Qbead.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 58cccdc..61a84b1 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -664,7 +664,7 @@ class Qbead {
   }
 
   void writeToBLE(BLECharacteristic& destination, Vector3d vector) {
-    float buffer[3] = {vector(0), vector(1), vector(2)};
+    float buffer[3] = {(float)vector(0), (float)vector(1), (float)vector(2)};
     destination.write(buffer, 3 * sizeof(float));
     for (uint16_t conn_hdl = 0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
     {

From 5a70758f9dbcead613c9f6c712f5b1393c97b166 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 20 May 2025 14:17:55 +0200
Subject: [PATCH 22/64] fixed issue when Serial wasn't available

---
 src/Qbead.h | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index e41f584..dcf76d2 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -455,7 +455,11 @@ class Qbead {
   void begin() {
     singletoninstance = this;
     Serial.begin(9600);
-    while (!Serial); // TODO some form of warning or a way to give up if Serial never becomes available
+    for (int waitCount = 0; waitCount < 50; waitCount++)
+    {
+      if (Serial) {break;}
+      delay(100);
+    }
 
     pixels.begin();
     clear();

From 3fd56ff4ad1ce0db8d5d9bdaebb940b70054e199 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 20 May 2025 15:01:33 +0200
Subject: [PATCH 23/64] Move animation to Qbead.h

---
 .../PauliGate_detection.ino                   | 83 ++++---------------
 src/Qbead.h                                   | 70 +++++++++++++++-
 2 files changed, 83 insertions(+), 70 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index d7b54d4..63e502d 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -1,70 +1,10 @@
 #include <Qbead.h>
 
 Qbead::Qbead bead;
-Qbead::QuantumState state = Qbead::QuantumState(Qbead::Coordinates(1, 0));
-int rotationState;
+int rotationState = 0;
 uint32_t stateColor = color(255, 255, 255);
 const bool toggleAnimationOn = 1;
 
-
-void animationGate(int gateType, int steps, int animationLength)
-{
-  if (gateType == 8)
-  {
-    state.collapse();
-    return;
-  }
-  if (!toggleAnimationOn)
-  {
-    steps = 1;
-  }
-  float stepLength = M_PI / (float) steps;
-  stateColor = color(255, 0, 255);
-  for (int i = 0; i < steps; i++)
-  {
-    bead.clear();
-    bead.showAxis();
-    switch (gateType)
-    {
-      case 1:
-        Serial.print("Executing X gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateX(-stepLength);
-        break;
-      case 2:
-        Serial.print("Executing Y gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateY(-stepLength);
-        break;
-      case 6:
-        Serial.print("Executing Z gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateZ(-stepLength);
-        break;
-      case 4:
-        Serial.print("Executing X gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateX(stepLength);
-        break;
-      case 5:
-        Serial.print("Executing Y gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateY(stepLength);
-        break;
-      case 3:
-        Serial.print("Executing Z gate, progress: ");
-        Serial.println(i/(float)steps);
-        state.gateZ(stepLength);
-        break;
-      case 7:
-        state.gateH(stepLength);
-    }
-    bead.setLed(state.getCoordinates(), stateColor);
-    bead.show();
-    delay(animationLength/steps);
-  }
-}
-
 void setup() {
   bead.begin();
   bead.setBrightness(25); // way too bright
@@ -86,15 +26,26 @@ void setup() {
 }
 
 void loop() {
-  bead.readIMU(true);
+  bead.readIMU(false);
   bead.clear();
   bead.showAxis();
   stateColor = color(255, 255, 255);
-  rotationState = bead.checkMotion();
-  if (rotationState != 0) 
+  Serial.print("rotationState: ");
+  Serial.println(rotationState);
+  if (!bead.frozen)
+  {
+    rotationState = bead.checkMotion();
+    if (rotationState != 0)
+    {
+      bead.frozen = true;
+      bead.T_freeze = millis();
+    }
+  }
+  else
   {
-    animationGate(rotationState, 10, 4000);
+    stateColor = color(122, 122, 0);
   }
-  bead.setLed(state.getCoordinates(), stateColor);
+  bead.animateTo(rotationState, 2000);
+  bead.setLed(bead.visualState, stateColor);
   bead.show();
 }
\ No newline at end of file
diff --git a/src/Qbead.h b/src/Qbead.h
index dcf76d2..c4debf6 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -286,12 +286,42 @@ class QuantumState
     stateCoordinates.set(2*acos(abs(stateVector.x())), arg(stateVector.y()) - arg(stateVector.x()));
   }
 
+  void applyGateType(uint16_t gateType, float rotationDegree = PI)
+  {
+    switch (gateType)
+    {
+    case 1:
+      gateX(-rotationDegree);
+      break;
+    case 2:
+      gateY(-rotationDegree);
+      break;
+    case 3:
+      gateZ(rotationDegree);
+      break;
+    case 4:
+      gateX(rotationDegree);
+      break;
+    case 5:
+      gateY(rotationDegree);
+      break;
+    case 6:
+      gateZ(-rotationDegree);
+      break;
+    case 7:
+      gateH(rotationDegree);
+      break;
+    default:
+      break;
+    }
+  }
+
   // Rotate PI around the x axis
   void gateX(float rotationDegree = PI)
   {
     Matrix2cf gateMatrix;
     gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f) * i,
-        -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // gloabal phase differs from pauli gates but thid doesn't matter for bloch sphere
+        -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // global phase differs from pauli gates but this doesn't matter for bloch sphere
     applyGate(gateMatrix);
   }
 
@@ -351,9 +381,13 @@ class Qbead {
 
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
-  Vector3d gravityVector = Vector3d(0, 0, 1); // gravity vector
-  Vector3d gyroVector = Vector3d(0, 0, 1); // gyro vector
-  float yaw;
+  float T_freeze = 0;
+  bool frozen = false; // frozen means that there is an animation in progress
+  QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
+  Coordinates visualState = Coordinates(-0.866, 0.25, -0.433);
+  Vector3d gravityVector = Vector3d(0, 0, 1);
+  Vector3d gyroVector = Vector3d(0, 0, 1);
+  float yaw = 0;
 
   float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble = 0xffffff; // color as sent over BLE connection
@@ -584,8 +618,35 @@ class Qbead {
     setBloch_deg(theta, phi, c, true);
   }
 
+  void animateTo(uint8_t gate, uint16_t animationLength = 2000)
+  {
+    if (!frozen || gate == 0)
+    {
+      return;
+    }
+    float T_new = millis();
+    float delta = T_new - T_freeze;
+    if (delta > animationLength)
+    {
+      frozen = false;
+      state.applyGateType(gate);
+      Serial.println("Animation finished");
+      return;
+    }
+    float d = delta * PI / float(animationLength);
+    QuantumState from = state;
+    from.applyGateType(gate, d);
+    visualState.set(from.getCoordinates().v);
+  }
+
   int checkMotion()
   {
+    if (frozen)
+    {
+      return 0;
+    }
+    frozen = true;
+    T_freeze = micros();
     // Handle tap interrupt
     if (interruptCount > prevInterruptCount)
     {
@@ -619,6 +680,7 @@ class Qbead {
         return i + 4; // 4 = x, 5 = y, 6 = -z
       }
     }
+    frozen = false;
     return 0;
   }
 

From 38c1a20d3f62e272012f5a1d9b1c29c4c7305804 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 20 May 2025 15:11:01 +0200
Subject: [PATCH 24/64] Fix collapsing

---
 examples/PauliGate_detection/PauliGate_detection.ino | 10 +++++-----
 src/Qbead.h                                          | 11 ++++++++++-
 2 files changed, 15 insertions(+), 6 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index 63e502d..c8dc102 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -32,7 +32,11 @@ void loop() {
   stateColor = color(255, 255, 255);
   Serial.print("rotationState: ");
   Serial.println(rotationState);
-  if (!bead.frozen)
+  if (bead.frozen)
+  {
+    stateColor = color(122, 122, 0);
+  }
+  else
   {
     rotationState = bead.checkMotion();
     if (rotationState != 0)
@@ -41,10 +45,6 @@ void loop() {
       bead.T_freeze = millis();
     }
   }
-  else
-  {
-    stateColor = color(122, 122, 0);
-  }
   bead.animateTo(rotationState, 2000);
   bead.setLed(bead.visualState, stateColor);
   bead.show();
diff --git a/src/Qbead.h b/src/Qbead.h
index c4debf6..9965f1a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -620,10 +620,19 @@ class Qbead {
 
   void animateTo(uint8_t gate, uint16_t animationLength = 2000)
   {
-    if (!frozen || gate == 0)
+    if (frozen)
+    {
+      prevInterruptCount = interruptCount;
+    }
+    else if (gate == 0)
     {
       return;
     }
+    if (gate == 8)
+    {
+      state.collapse();
+      visualState.set(state.getCoordinates().v);
+    }
     float T_new = millis();
     float delta = T_new - T_freeze;
     if (delta > animationLength)

From cadbfb5550c05371e97760fd2743d7f80e1232b1 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Thu, 22 May 2025 11:18:26 +0200
Subject: [PATCH 25/64] Make smooth function more generic

---
 src/Qbead.h | 68 ++++++++++++++++++++++++++---------------------------
 1 file changed, 34 insertions(+), 34 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 9965f1a..c38338b 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -53,11 +53,11 @@ static uint8_t redch(uint32_t rgb) {
 }
 
 static uint8_t greench(uint32_t rgb) {
-  return (0x00ff00 & rgb) >> 8;
+  return 0x0000ff & rgb;
 }
 
 static uint8_t bluech(uint32_t rgb) {
-  return 0x0000ff & rgb;
+  return (0x00ff00 & rgb) >> 8;
 }
 
 uint32_t colorWheel(uint8_t wheelPos) {
@@ -554,13 +554,13 @@ class Qbead {
     pixels.setBrightness(b);
   }
 
-  void setLed(Coordinates coordinates, uint32_t color, bool smooth = false) {
+  void setLed(Coordinates coordinates, uint32_t color, int leds = 1) {
     float theta = coordinates.theta() * 180 / PI;
     float phi = coordinates.phi() * 180 / PI;
     if (phi < 0) {
       phi += 360;
     }
-    setBloch_deg(theta, phi, color, smooth);
+    setBloch_deg(theta, phi, color, leds);
   }
 
   void showAxis() {
@@ -580,42 +580,42 @@ class Qbead {
   }
 
   // Single bit is lit up on the Bloch sphere  
-  void setBloch_deg(float theta, float phi, uint32_t c, bool smooth = false) {
-    int closest_index = -1;
-    float closest_dist = 1000;
-    int second_closest_index = -1;
-    float second_closest_dist = 1000;
-    for (int i = 0; i < 62; i++) {
-      float dist = getDistToLed(theta * PI / 180, phi * PI / 180, i);
-      if (dist < closest_dist) {
-        second_closest_index = closest_index;
-        second_closest_dist = closest_dist;
-        closest_index = i;
-        closest_dist = dist;
-      } else if (dist < second_closest_dist) {
-        second_closest_index = i;
-        second_closest_dist = dist;
+  void setBloch_deg(float theta, float phi, uint32_t c, int leds = 1) {
+    int index[leds];
+    float dist[leds];
+    for (int i = 0; i < leds; i++) {
+      index[i] = -1;
+      dist[i] = 1000;
+    }
+    for (int i = 0; i < QB_PIXEL_COUNT; i++) {
+      float d = getDistToLed(theta * PI / 180, phi * PI / 180, i);
+      for (int j = 0; j < leds; j++) {
+        if (d < dist[j]) {
+          for (int k = leds - 1; k > j; k--) {
+            index[k] = index[k - 1];
+            dist[k] = dist[k - 1];
+          }
+          index[j] = i;
+          dist[j] = d;
+          break;
+        }
       }
     }
-    if (smooth) {
-      float dist1 = getDistToLed(theta * PI / 180, phi * PI / 180, closest_index);
-      float dist2 = getDistToLed(theta * PI / 180, phi * PI / 180, second_closest_index);
-      float ratio1 = dist1 / (dist1 + dist2);
-      float ratio2 = dist2 / (dist1 + dist2);
-      uint8_t r = redch(c);
-      uint8_t g = greench(c);
-      uint8_t b = bluech(c);
-      float p1 = ratio1 * ratio1;
-      float p2 = ratio2 * ratio2;
-      pixels.setPixelColor(closest_index, color(p2 * r, p2 * g, p2 * b));
-      pixels.setPixelColor(second_closest_index, color(p1 * r, p1 * g, p1 * b));
-    } else {
-      pixels.setPixelColor(closest_index, c);
+    float min_dist = dist[0];
+    for (int i = 0; i < leds; i++) {
+      if (index[i] != -1) {
+        uint8_t r = redch(c);
+        uint8_t g = greench(c);
+        uint8_t b = bluech(c);
+        float p = dist[i] / min_dist;
+        float p2 = p * p;
+        pixels.setPixelColor(index[i], color(p2 * r, p2 * g, p2 * b));
+      }
     }
   }
 
   void setBloch_deg_smooth(float theta, float phi, uint32_t c) {
-    setBloch_deg(theta, phi, c, true);
+    setBloch_deg(theta, phi, c, 2);
   }
 
   void animateTo(uint8_t gate, uint16_t animationLength = 2000)

From e290e776fcd84f859f31ad63ae39424f09bfc891 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Thu, 22 May 2025 12:16:44 +0200
Subject: [PATCH 26/64] Improve smoothness

---
 src/Qbead.h | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index c38338b..138dd6c 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -601,14 +601,12 @@ class Qbead {
         }
       }
     }
-    float min_dist = dist[0];
     for (int i = 0; i < leds; i++) {
       if (index[i] != -1) {
         uint8_t r = redch(c);
         uint8_t g = greench(c);
         uint8_t b = bluech(c);
-        float p = dist[i] / min_dist;
-        float p2 = p * p;
+        float p2 =  pow(200, -dist[i]);
         pixels.setPixelColor(index[i], color(p2 * r, p2 * g, p2 * b));
       }
     }

From 9a46f5f13709ec747b9d6f7f3869f3e95409d99a Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Thu, 22 May 2025 12:36:48 +0200
Subject: [PATCH 27/64] Show decoherence in an example

---
 examples/Decoherence/Decoherence.ino | 67 ++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)
 create mode 100644 examples/Decoherence/Decoherence.ino

diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino
new file mode 100644
index 0000000..70fcb90
--- /dev/null
+++ b/examples/Decoherence/Decoherence.ino
@@ -0,0 +1,67 @@
+#include <Qbead.h>
+
+Qbead::Qbead bead;
+int rotationState = 0;
+uint32_t stateColor = color(255, 255, 255);
+const bool toggleAnimationOn = 1;
+int lastCollapseTime = 0;
+
+void setup()
+{
+    bead.begin();
+    bead.setBrightness(25);
+    Serial.println("testing all pixels discretely");
+    for (int i = 0; i < bead.pixels.numPixels(); i++)
+    {
+        bead.pixels.setPixelColor(i, color(255, 255, 255));
+        bead.pixels.show();
+        delay(5);
+    }
+    Serial.println("testing smooth transition between pixels");
+    for (int phi = 0; phi < 360; phi += 30)
+    {
+        for (int theta = 0; theta < 180; theta += 3)
+        {
+            bead.clear();
+            bead.setBloch_deg(theta, phi, colorWheel_deg(phi));
+            bead.show();
+        }
+    }
+    Serial.println("starting inertial tracking");
+    lastCollapseTime = millis();
+}
+
+void loop()
+{
+    bead.readIMU(false);
+    bead.clear();
+    bead.showAxis();
+    stateColor = color(255, 255, 255);
+    Serial.print("rotationState: ");
+    Serial.println(rotationState);
+    if (bead.frozen)
+    {
+        stateColor = color(122, 122, 0);
+    }
+    else
+    {
+        rotationState = bead.checkMotion();
+        if (rotationState != 0)
+        {
+            bead.frozen = true;
+            bead.T_freeze = millis();
+        }
+        if (rotationState == 8)
+        {
+            lastCollapseTime = millis();
+        }
+    }
+    int currentTime = millis();
+    // Increment the amount of leds based on the time since the last collapse
+    // This is to show decoherence
+    int leds = (currentTime - lastCollapseTime) / 2000;
+    leds = constrain(leds, 1, 15);
+    bead.animateTo(rotationState, 2000);
+    bead.setLed(bead.visualState, stateColor, leds);
+    bead.show();
+}
\ No newline at end of file

From b62c22a15d91a9eb5def5910bd5c734fb251297d Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 23 May 2025 10:08:25 +0200
Subject: [PATCH 28/64] shaking function

---
 src/Qbead.h | 59 +++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 59 insertions(+)

diff --git a/src/Qbead.h b/src/Qbead.h
index 9965f1a..de09a05 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -382,7 +382,10 @@ class Qbead {
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
+  float T_shaking = 0;
+  float shakingCounter = 0;
   bool frozen = false; // frozen means that there is an animation in progress
+  bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
   Coordinates visualState = Coordinates(-0.866, 0.25, -0.433);
   Vector3d gravityVector = Vector3d(0, 0, 1);
@@ -628,6 +631,10 @@ class Qbead {
     {
       return;
     }
+    if (gate == 9)
+    {
+      visualState.set(state.getCoordinates().v);
+    }
     if (gate == 8)
     {
       state.collapse();
@@ -648,6 +655,48 @@ class Qbead {
     visualState.set(from.getCoordinates().v);
   }
 
+  bool detectShaking()
+  {
+    float totalAcceleration = gravityVector.norm();
+    Serial.println(totalAcceleration);
+    if (shakingState)
+    {
+      float newTime = millis();
+      shakingCounter += newTime - T_shaking;
+      T_shaking = newTime;
+      if (shakingCounter < 300)
+      {
+        return false;
+      }
+      if (totalAcceleration > 11)
+      {
+        Serial.println("RANDOMISING");
+        float randomTheta = (random(0, 1000)/1000.0f) * PI;
+        float randomPhi = (random(0, 1000)/500.0f) * PI;
+        state.setCoordinates(Coordinates(randomTheta, randomPhi));
+        setLed(state.getCoordinates(), color(255, 0, 255));
+        show();
+        shakingState = false;
+        prevInterruptCount = interruptCount;
+        return true;
+      }
+      if (shakingCounter > 800)
+      {
+        shakingState = false;
+      }
+      return false;
+    }
+    if (totalAcceleration > 11)
+    {
+      Serial.print("Detected shaking turning on shakingState, acc length: ");
+      Serial.println(totalAcceleration);
+      shakingState = true;
+      T_shaking = millis();
+      shakingCounter = 0;
+    } 
+    return false;
+  }
+
   int checkMotion()
   {
     if (frozen)
@@ -656,6 +705,16 @@ class Qbead {
     }
     frozen = true;
     T_freeze = micros();
+    Serial.print(shakingState); 
+    if (detectShaking())
+    {
+      return 9;
+    }
+    if (shakingState)
+    {
+      frozen = false;
+      return 0;
+    }
     // Handle tap interrupt
     if (interruptCount > prevInterruptCount)
     {

From 26a07045fc36e90773b7f3ad5caebca9a0677221 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Fri, 23 May 2025 11:54:34 +0200
Subject: [PATCH 29/64] removed print statements

---
 src/Qbead.h | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index de09a05..871889a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -658,7 +658,6 @@ class Qbead {
   bool detectShaking()
   {
     float totalAcceleration = gravityVector.norm();
-    Serial.println(totalAcceleration);
     if (shakingState)
     {
       float newTime = millis();
@@ -705,7 +704,6 @@ class Qbead {
     }
     frozen = true;
     T_freeze = micros();
-    Serial.print(shakingState); 
     if (detectShaking())
     {
       return 9;

From 3046e214466ff38e2ca2349808584bbc5eb4e764 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 26 May 2025 14:56:24 +0200
Subject: [PATCH 30/64] Fix collapsing

---
 src/Qbead.h | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 4f87979..7d8253a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -273,8 +273,15 @@ class QuantumState
   void collapse()
   {
     const float theta = stateCoordinates.theta();
-    const float a = cos(theta / 2);
-    const bool is1 = random(0, 100) < a * a * 100;
+    const float a = (cos(theta) + 1) / 2; // probability of measuring |0>
+    if (a < 0.0001) {
+      stateCoordinates.set(0, 0, -1);
+      return;
+    } else if (a > 0.9999) {
+      stateCoordinates.set(0, 0, 1);
+      return;
+    }
+    const bool is1 = random(0, 100) <= a * a * 100;
     this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
   }
 

From 153ce2c467a3fa6bd95a324b0088c579dd5dd74b Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Mon, 26 May 2025 15:16:00 +0200
Subject: [PATCH 31/64] create new src file for ESP32 version

---
 src_ESP32/QbeadESP32.h | 846 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 846 insertions(+)
 create mode 100644 src_ESP32/QbeadESP32.h

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
new file mode 100644
index 0000000..333d599
--- /dev/null
+++ b/src_ESP32/QbeadESP32.h
@@ -0,0 +1,846 @@
+#ifndef QBEAD_H
+#define QBEAD_H
+
+#include <Arduino.h>
+#include <Adafruit_NeoPixel.h>
+#include <LSM6DS3.h>
+#include <math.h>
+#include <ArduinoEigen.h>
+
+using namespace Eigen;
+
+// default configs
+#define QB_LEDPIN 10
+#define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
+#define QB_IMU_ADDR 0x6A
+#define QB_IX 1
+#define QB_IY 0
+#define QB_IZ 2
+#define QB_SX 0
+#define QB_SY 0
+#define QB_SZ 1
+#define GYRO_MEASUREMENT_THRESHOLD 0.0001
+#define GYRO_GATE_THRESHOLD 7
+#define SHAKING_THRESHOLD 0.09
+#define QB_PIXEL_COUNT 62
+#define QB_MAX_PRPH_CONNECTION 2
+#define T_ACC 100000
+#define T_GYRO 10000
+
+const uint8_t QB_UUID_SERVICE[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
+const uint8_t QB_UUID_COL_CHAR[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3};
+const uint8_t QB_UUID_SPH_CHAR[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
+const uint8_t QB_UUID_ACC_CHAR[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
+const uint8_t QB_UUID_GYR_CHAR[] =
+{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
+
+const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+const std::complex<float>i(0, 1);
+
+// TODO manage namespaces better
+// The setPixelColor switches blue and green
+static uint32_t color(uint8_t r, uint8_t g, uint8_t b) {
+  return ((uint32_t)r << 16) | ((uint16_t)b << 8) | g;
+}
+
+static uint8_t redch(uint32_t rgb) {
+  return rgb >> 16;
+}
+
+static uint8_t greench(uint32_t rgb) {
+  return 0x0000ff & rgb;
+}
+
+static uint8_t bluech(uint32_t rgb) {
+  return (0x00ff00 & rgb) >> 8;
+}
+
+uint32_t colorWheel(uint8_t wheelPos) {
+  wheelPos = 255 - wheelPos;
+  if (wheelPos < 85) {
+    return color(255 - wheelPos * 3, 0, wheelPos * 3);
+  }
+  if (wheelPos < 170) {
+    wheelPos -= 85;
+    return color(0, wheelPos * 3, 255 - wheelPos * 3);
+  }
+  wheelPos -= 170;
+  return color(wheelPos * 3, 255 - wheelPos * 3, 0);
+}
+
+uint32_t colorWheel_deg(float wheelPos) {
+  return colorWheel(wheelPos * 255 / 360);
+}
+
+float sign(float x) {
+  if (x > 0) return +1;
+  else return -1;
+}
+
+// z = cos(t)
+// x = cos(p)sin(t)
+// y = sin(p)sin(t)
+// Return the angle in radians between the x-axis and the line to the point (x, y)
+float phi(float x, float y) {
+  return atan2(y, x);
+}
+
+float phi(float x, float y, float z) {
+  return phi(x, y);
+}
+
+float theta(float x, float y, float z) {
+  float ll = x * x + y * y + z * z;
+  float l = sqrt(ll);
+  float theta = acos(z / l);
+  return theta;
+}
+
+bool checkThetaAndPhi(float theta, float phi) {
+  return theta >= 0 && theta <= 180 && phi >= 0 && phi <= 360;
+}
+
+void connect_callback(uint16_t conn_handle)
+{
+  // Get the reference to current connection
+  BLEConnection* connection = Bluefruit.Connection(conn_handle);
+
+  char central_name[32] = { 0 };
+  connection->getPeerName(central_name, sizeof(central_name));
+
+  Serial.print("[INFO]{BLE} Connected to "); // TODO take care of cases where Serial is not available
+  Serial.println(central_name);
+}
+
+// In rads
+void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
+{
+  // Normalize yaw to be between 0 and 2*PI
+  phi = fmod(phi, 2 * PI);
+  if (phi < 0)
+  {
+    phi += 2 * PI;
+  }
+  if (!checkThetaAndPhi(theta * 180 / PI, phi * 180 / PI))
+  {
+    Serial.print("Theta or Phi out of range when creating coordinates class, initializing as 1");
+    Serial.print("Theta: ");
+    Serial.print(theta);
+    Serial.print("Phi: ");
+    Serial.println(phi);
+    x = 0;
+    y = 0;
+    z = 1;
+    return;
+  }
+
+  x = sin(theta) * cos(phi);
+  y = sin(theta) * sin(phi);
+  z = cos(theta);
+}
+
+// Tap detection
+LSM6DS3 myIMU(I2C_MODE, 0x6A);
+uint8_t interruptCount = 0; // Amount of received interrupts
+uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
+
+void setupTapInterrupt() {
+  uint8_t error = 0;
+  uint8_t dataToWrite = 0;
+
+  // Double Tap Config
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
+  myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
+}
+
+void int1ISR()
+{
+  interruptCount++;
+}
+
+namespace Qbead {
+
+class Coordinates
+{
+public:
+  Vector3d v;
+
+  Coordinates(float argx, float argy, float argz)
+  {
+    v = Vector3d(argx, argy, argz);
+    v.normalize();
+  }
+
+  // In rads
+  Coordinates(float theta, float phi)
+  {
+    float x, y, z = 0;
+    sphericalToCartesian(theta, phi, x, y, z);
+    v = Vector3d(x, y, z);
+  }
+
+  Coordinates(Vector3d vector)
+  {
+    v = vector;
+    v.normalize();
+  }
+
+  // In rads
+  float theta()
+  {
+    return acos(v(2));
+  }
+
+  // In rads
+  float phi()
+  {
+    return atan2(v(1), v(0));
+  }
+
+  Vector2cf stateVector2D()
+  {
+    std::complex<float> alpha = cos(theta()/2);
+    std::complex<float> beta = exp(i*phi()) * sin(theta()/2);
+    return {alpha, beta};
+  }
+
+  float dist(Vector3d other) const
+  {
+    Vector3d diff = v - other;
+    return diff.norm();
+  }
+
+  void set(float argx, float argy, float argz)
+  {
+    v = Vector3d(argx, argy, argz);
+    v.normalize();
+  }
+
+  // in rads
+  void set(float theta, float phi)
+  {
+    float x, y, z = 0;
+    sphericalToCartesian(theta, phi, x, y, z);
+    v = Vector3d(x, y, z);
+  }
+
+  void set(Vector3d vector) {
+    v = vector;
+    v.normalize();
+  }
+
+  // in rads
+  void setTheta(float theta)
+  {
+    set(theta, phi());
+  }
+
+  // in rads
+  void setPhi(float phi)
+  {
+    set(theta(), phi);
+  }
+};
+
+class QuantumState
+{
+private:
+  Coordinates stateCoordinates;
+
+public:
+  QuantumState(Coordinates argStateCoordinates) : stateCoordinates(argStateCoordinates) {}
+  QuantumState() : stateCoordinates(0, 0, 1) {}
+
+  void setCoordinates(Coordinates argStateCoordinates)
+  {
+    stateCoordinates.set(argStateCoordinates.v);
+  }
+
+  Coordinates getCoordinates()
+  {
+    return stateCoordinates;
+  }
+
+  void collapse()
+  {
+    const float theta = stateCoordinates.theta();
+    const float a = cos(theta / 2);
+    const bool is1 = random(0, 100) < a * a * 100;
+    this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
+  }
+
+  void applyGate(Matrix2cf gate)
+  {
+    Vector2cf stateVector = stateCoordinates.stateVector2D();
+    stateVector = gate * stateVector;
+    stateVector.normalize();
+    stateCoordinates.set(2*acos(abs(stateVector.x())), arg(stateVector.y()) - arg(stateVector.x()));
+  }
+
+  void applyGateType(uint16_t gateType, float rotationDegree = PI)
+  {
+    switch (gateType)
+    {
+    case 1:
+      gateX(-rotationDegree);
+      break;
+    case 2:
+      gateY(-rotationDegree);
+      break;
+    case 3:
+      gateZ(rotationDegree);
+      break;
+    case 4:
+      gateX(rotationDegree);
+      break;
+    case 5:
+      gateY(rotationDegree);
+      break;
+    case 6:
+      gateZ(-rotationDegree);
+      break;
+    case 7:
+      gateH(rotationDegree);
+      break;
+    default:
+      break;
+    }
+  }
+
+  // Rotate PI around the x axis
+  void gateX(float rotationDegree = PI)
+  {
+    Matrix2cf gateMatrix;
+    gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f) * i,
+        -sin(rotationDegree / 2.0f) * i, cos(rotationDegree / 2.0f); // global phase differs from pauli gates but this doesn't matter for bloch sphere
+    applyGate(gateMatrix);
+  }
+
+  // Rotate PI around the y axis
+  void gateZ(float rotationDegree = PI)
+  {
+    Matrix2cf gateMatrix;
+    gateMatrix << exp(-i * rotationDegree / 2.0f), 0,
+        0, exp(i * rotationDegree / 2.0f);
+    applyGate(gateMatrix);
+  }
+
+  // Rotate PI around the z axis
+  void gateY(float rotationDegree = PI)
+  {
+    Matrix2cf gateMatrix;
+    gateMatrix << cos(rotationDegree / 2.0f), -sin(rotationDegree / 2.0f),
+        sin(rotationDegree / 2.0f), cos(rotationDegree / 2.0f);
+    applyGate(gateMatrix);
+  }
+
+  // Rotate PI around the xz axis
+  void gateH(float rotationDegree = PI)
+  {
+    Matrix2cf gateMatrix;
+    gateMatrix << (cos(rotationDegree / 2.0f) - i * sin(rotationDegree / 2.0f) / sqrt(2.0f)), -i * sin(rotationDegree / 2.0f) / sqrt(2.0f),
+        -i * sin(rotationDegree / 2.0f) / sqrt(2.0f), (cos(rotationDegree / 2.0f) + i * sin(rotationDegree / 2.0f) / sqrt(2.0f));
+    applyGate(gateMatrix);
+  }
+};
+
+class Qbead {
+public:
+  Qbead(const uint16_t pin00 = QB_LEDPIN,
+        const uint16_t pixelconfig = QB_PIXELCONFIG,
+        const uint8_t imu_addr = QB_IMU_ADDR)
+      : imu(LSM6DS3(I2C_MODE, imu_addr)),
+        pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)),
+        bleservice(QB_UUID_SERVICE),
+        blecharcol(QB_UUID_COL_CHAR),
+        blecharsph(QB_UUID_SPH_CHAR),
+        blecharacc(QB_UUID_ACC_CHAR),
+        blechargyr(QB_UUID_GYR_CHAR)
+  {}
+
+  static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
+
+  LSM6DS3 imu;
+  Adafruit_NeoPixel pixels;
+
+  BLEService bleservice;
+  BLECharacteristic blecharcol;
+  BLECharacteristic blecharsph;
+  BLECharacteristic blecharacc;
+  BLECharacteristic blechargyr;
+  uint8_t connection_count = 0;
+
+  float rbuffer[3], rgyrobuffer[3];
+  float T_imu;             // last update from the IMU
+  float T_freeze = 0;
+  float T_shaking = 0;
+  float shakingCounter = 0;
+  bool frozen = false; // frozen means that there is an animation in progress
+  bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
+  QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
+  Coordinates visualState = Coordinates(-0.866, 0.25, -0.433);
+  Vector3d gravityVector = Vector3d(0, 0, 1);
+  Vector3d gyroVector = Vector3d(0, 0, 1);
+  float yaw = 0;
+
+  float t_ble, p_ble; // theta and phi as sent over BLE connection
+  uint32_t c_ble = 0xffffff; // color as sent over BLE connection
+
+  // led map index to Coordinates
+  // This map is for the first version of the pcb
+  Coordinates led_map_v1[62] = {
+    Coordinates(-1, -0, -0),
+    Coordinates(-0.866, 0, -0.5),
+    Coordinates(-0.5, 0, -0.866),
+    Coordinates(-0, 0, -1),
+    Coordinates(0.5, 0, -0.866),
+    Coordinates(0.866, 0, -0.5),
+    Coordinates(1, 0, 0),
+    Coordinates(-0.866, 0.25, -0.433),
+    Coordinates(-0.5, 0.433, -0.75),
+    Coordinates(-0, 0.5, -0.866),
+    Coordinates(0.5, 0.433, -0.75),
+    Coordinates(0.866, 0.25, -0.433),
+    Coordinates(-0.866, 0.433, -0.25),
+    Coordinates(-0.5, 0.75, -0.433),
+    Coordinates(-0, 0.866, -0.5),
+    Coordinates(0.5, 0.75, -0.433),
+    Coordinates(0.866, 0.433, -0.25),
+    Coordinates(-0.866, 0.5, 0),
+    Coordinates(-0.5, 0.866, 0),
+    Coordinates(-0, 1, 0),
+    Coordinates(0.5, 0.866, 0),
+    Coordinates(0.866, 0.5, 0),
+    Coordinates(-0.866, 0.433, 0.25),
+    Coordinates(-0.5, 0.75, 0.433),
+    Coordinates(-0, 0.866, 0.5),
+    Coordinates(0.5, 0.75, 0.433),
+    Coordinates(0.866, 0.433, 0.25),
+    Coordinates(-0.866, 0.25, 0.433),
+    Coordinates(-0.5, 0.433, 0.75),
+    Coordinates(-0, 0.5, 0.866),
+    Coordinates(0.5, 0.433, 0.75),
+    Coordinates(0.866, 0.25, 0.433),
+    Coordinates(-0.866, -0, 0.5),
+    Coordinates(-0.5, -0, 0.866),
+    Coordinates(-0, -0, 1),
+    Coordinates(0.5, -0, 0.866),
+    Coordinates(0.866, -0, 0.5),
+    Coordinates(-0.866, -0.25, 0.433),
+    Coordinates(-0.5, -0.433, 0.75),
+    Coordinates(-0, -0.5, 0.866),
+    Coordinates(0.5, -0.433, 0.75),
+    Coordinates(0.866, -0.25, 0.433),
+    Coordinates(-0.866, -0.433, 0.25),
+    Coordinates(-0.5, -0.75, 0.433),
+    Coordinates(-0, -0.866, 0.5),
+    Coordinates(0.5, -0.75, 0.433),
+    Coordinates(0.866, -0.433, 0.25),
+    Coordinates(-0.866, -0.5, -0),
+    Coordinates(-0.5, -0.866, -0),
+    Coordinates(-0, -1, -0),
+    Coordinates(0.5, -0.866, -0),
+    Coordinates(0.866, -0.5, -0),
+    Coordinates(-0.866, -0.433, -0.25),
+    Coordinates(-0.5, -0.75, -0.433),
+    Coordinates(-0, -0.866, -0.5),
+    Coordinates(0.5, -0.75, -0.433),
+    Coordinates(0.866, -0.433, -0.25),
+    Coordinates(-0.866, -0.25, -0.433),
+    Coordinates(-0.5, -0.433, -0.75),
+    Coordinates(-0, -0.5, -0.866),
+    Coordinates(0.5, -0.433, -0.75),
+    Coordinates(0.866, -0.25, -0.433),
+  };
+
+  static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
+    Serial.println("[INFO]{BLE} Received a write on the color characteristic");
+    singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
+    Serial.print("[DEBUG]{BLE} Received");
+    Serial.println(singletoninstance->c_ble, HEX);
+  }
+
+  static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len){
+    Serial.println("[INFO]{BLE} Received a write on the spherical coordinates characteristic");
+    singletoninstance->t_ble = ((uint32_t)data[0])*180/255;
+    singletoninstance->p_ble = ((uint32_t)data[1])*360/255;
+    Serial.print("[DEBUG]{BLE} Received t=");
+    Serial.print(singletoninstance->t_ble);
+    Serial.print(" p=");
+    Serial.println(singletoninstance->p_ble);
+  }
+
+  void startAccelerometer() {
+    // BLE Characteristic IMU xyz accelerometer readout
+    blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
+    blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blecharacc.setUserDescriptor("xyz acceleration");
+    blecharacc.setFixedLen(3*sizeof(float));
+    blecharacc.begin();
+    blecharacc.write(zerobuffer20, 3*sizeof(float));    
+  }
+
+  void begin() {
+    singletoninstance = this;
+    Serial.begin(9600);
+    for (int waitCount = 0; waitCount < 50; waitCount++)
+    {
+      if (Serial) {break;}
+      delay(100);
+    }
+
+    pixels.begin();
+    clear();
+    setBrightness(10);
+
+    Serial.println("[INFO] Booting... Qbead on XIAO BLE Sense + LSM6DS3 compiled on " __DATE__ " at " __TIME__);
+    if (!imu.begin()) {
+      Serial.println("[DEBUG]{IMU} IMU initialized correctly");
+    } else {
+      Serial.println("[ERROR]{IMU} IMU failed to initialize");
+    }
+
+    // BLE Peripheral service setup
+    Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
+    Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
+    Bluefruit.Periph.setConnectCallback(connect_callback);
+    bleservice.begin();
+    // BLE Characteristic Bloch Sphere Visualizer color setup
+    blecharcol.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
+    blecharcol.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blecharcol.setUserDescriptor("BSV rgb color");
+    blecharcol.setFixedLen(3);
+    blecharcol.setWriteCallback(ble_callback_color);
+    blecharcol.begin();
+    blecharcol.write(zerobuffer20, 3);
+    // BLE Characteristic Bloch Sphere Visualizer spherical coordinate setup
+    blecharsph.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
+    blecharsph.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blecharsph.setUserDescriptor("BSV spherical coordinates");
+    blecharsph.setFixedLen(2);
+    blecharsph.setWriteCallback(ble_callback_theta_phi);
+    blecharsph.begin();
+    blecharsph.write(zerobuffer20, 2);
+    // BLE Characteristic IMU xyz gyroscope readout
+    blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
+    blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN);
+    blechargyr.setUserDescriptor("xyz gyroscope");
+    blechargyr.setFixedLen(3*sizeof(float));
+    blechargyr.begin();
+    blechargyr.write(zerobuffer20, 3*sizeof(float));
+    startBLEadv();
+
+    // Tap detection
+    setupTapInterrupt();
+    pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
+    attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
+  }
+
+  void clear() {
+    pixels.clear();
+  }
+
+  void show() {
+    pixels.show();
+  }
+
+  void setBrightness(uint8_t b) {
+    pixels.setBrightness(b);
+  }
+
+  void setLed(Coordinates coordinates, uint32_t color, int leds = 1) {
+    float theta = coordinates.theta() * 180 / PI;
+    float phi = coordinates.phi() * 180 / PI;
+    if (phi < 0) {
+      phi += 360;
+    }
+    setBloch_deg(theta, phi, color, leds);
+  }
+
+  void showAxis() {
+    setLed(Coordinates(1, 0, 0), color(0, 0, 122));
+    setLed(Coordinates(-1, 0, 0), color(0, 0, 122));
+    setLed(Coordinates(0, 1, 0), color(0, 0, 122));
+    setLed(Coordinates(0, -1, 0), color(0, 0, 122));
+    setLed(Coordinates(0, 0, 1), color(0, 255, 0));
+    setLed(Coordinates(0, 0, -1), color(255, 0, 0));
+  }
+
+  // in rads
+  float getDistToLed(float theta, float phi, int index) {
+    const Coordinates led = led_map_v1[index];
+    const Coordinates reference(theta, phi);
+    return led.dist(reference.v);
+  }
+
+  // Single bit is lit up on the Bloch sphere  
+  void setBloch_deg(float theta, float phi, uint32_t c, int leds = 1) {
+    int index[leds];
+    float dist[leds];
+    for (int i = 0; i < leds; i++) {
+      index[i] = -1;
+      dist[i] = 1000;
+    }
+    for (int i = 0; i < QB_PIXEL_COUNT; i++) {
+      float d = getDistToLed(theta * PI / 180, phi * PI / 180, i);
+      for (int j = 0; j < leds; j++) {
+        if (d < dist[j]) {
+          for (int k = leds - 1; k > j; k--) {
+            index[k] = index[k - 1];
+            dist[k] = dist[k - 1];
+          }
+          index[j] = i;
+          dist[j] = d;
+          break;
+        }
+      }
+    }
+    for (int i = 0; i < leds; i++) {
+      if (index[i] != -1) {
+        uint8_t r = redch(c);
+        uint8_t g = greench(c);
+        uint8_t b = bluech(c);
+        float p2 =  pow(200, -dist[i]);
+        pixels.setPixelColor(index[i], color(p2 * r, p2 * g, p2 * b));
+      }
+    }
+  }
+
+  void setBloch_deg_smooth(float theta, float phi, uint32_t c) {
+    setBloch_deg(theta, phi, c, 2);
+  }
+
+  void animateTo(uint8_t gate, uint16_t animationLength = 2000)
+  {
+    if (frozen)
+    {
+      prevInterruptCount = interruptCount;
+    }
+    else if (gate == 0)
+    {
+      return;
+    }
+    if (gate == 9)
+    {
+      visualState.set(state.getCoordinates().v);
+    }
+    if (gate == 8)
+    {
+      state.collapse();
+      visualState.set(state.getCoordinates().v);
+    }
+    float T_new = millis();
+    float delta = T_new - T_freeze;
+    if (delta > animationLength)
+    {
+      frozen = false;
+      state.applyGateType(gate);
+      Serial.println("Animation finished");
+      return;
+    }
+    float d = delta * PI / float(animationLength);
+    QuantumState from = state;
+    from.applyGateType(gate, d);
+    visualState.set(from.getCoordinates().v);
+  }
+
+  bool detectShaking()
+  {
+    float totalAcceleration = gravityVector.norm();
+    if (shakingState)
+    {
+      float newTime = millis();
+      shakingCounter += newTime - T_shaking;
+      T_shaking = newTime;
+      if (shakingCounter < 300)
+      {
+        return false;
+      }
+      if (totalAcceleration > 11)
+      {
+        Serial.println("RANDOMISING");
+        float randomTheta = (random(0, 1000)/1000.0f) * PI;
+        float randomPhi = (random(0, 1000)/500.0f) * PI;
+        state.setCoordinates(Coordinates(randomTheta, randomPhi));
+        setLed(state.getCoordinates(), color(255, 0, 255));
+        show();
+        shakingState = false;
+        prevInterruptCount = interruptCount;
+        return true;
+      }
+      if (shakingCounter > 800)
+      {
+        shakingState = false;
+      }
+      return false;
+    }
+    if (totalAcceleration > 11)
+    {
+      Serial.print("Detected shaking turning on shakingState, acc length: ");
+      Serial.println(totalAcceleration);
+      shakingState = true;
+      T_shaking = millis();
+      shakingCounter = 0;
+    } 
+    return false;
+  }
+
+  int checkMotion()
+  {
+    if (frozen)
+    {
+      return 0;
+    }
+    frozen = true;
+    T_freeze = micros();
+    if (detectShaking())
+    {
+      return 9;
+    }
+    if (shakingState)
+    {
+      frozen = false;
+      return 0;
+    }
+    // Handle tap interrupt
+    if (interruptCount > prevInterruptCount)
+    {
+      uint8_t tapStatus = 0;
+      myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
+      prevInterruptCount = interruptCount;
+
+      if (tapStatus & 0x01)
+      {
+        Serial.println("Collapsing");
+        return 8;
+      }
+      else
+      {
+        Serial.println("Executing H gate");
+        return 7;
+      }
+    }
+    // Handle shaking
+    for (int i = 0; i < 3; i++)
+    {
+      if (gyroVector[i] > GYRO_GATE_THRESHOLD)
+      {
+        return i + 1; // 1 = -x, 2 = -y, 3 = z
+      }
+    }
+    for (int i = 0; i < 3; i++)
+    {
+      if (gyroVector[i] < - GYRO_GATE_THRESHOLD)
+      {
+        return i + 4; // 4 = x, 5 = y, 6 = -z
+      }
+    }
+    frozen = false;
+    return 0;
+  }
+
+  void writeToBLE(BLECharacteristic& destination, Vector3d vector) {
+    float buffer[3] = {(float)vector(0), (float)vector(1), (float)vector(2)};
+    destination.write(buffer, 3 * sizeof(float));
+    for (uint16_t conn_hdl = 0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
+    {
+      if (Bluefruit.connected(conn_hdl) && destination.notifyEnabled(conn_hdl))
+      {
+        destination.notify(buffer, 3 * sizeof(float));
+      }
+    }
+  }
+
+  Vector3d getVectorFromBuffer(float *buffer) {
+    // calibration of imu because imu is not aligned with bloch sphere
+    float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
+    float ry = (1 - 2 * QB_SY) * buffer[QB_IY];
+    float rz = (1 - 2 * QB_SZ) * buffer[QB_IZ];
+    return Vector3d(rx, ry, rz);
+  }
+
+  void readIMU(bool print=true) {
+    rbuffer[0] = imu.readFloatAccelX();
+    rbuffer[1] = imu.readFloatAccelY();
+    rbuffer[2] = imu.readFloatAccelZ();    
+    rgyrobuffer[0] = imu.readFloatGyroX();
+    rgyrobuffer[1] = imu.readFloatGyroY();
+    rgyrobuffer[2] = imu.readFloatGyroZ();
+
+    float T_new = micros();
+    float delta = T_new - T_imu;
+    T_imu = T_new;
+
+    Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
+    float d = min(delta / float(T_GYRO), 1.0f);
+    gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
+
+    Vector3d newGravity = getVectorFromBuffer(rbuffer);
+    d = min(delta / float(T_ACC), 1.0f);
+    gravityVector = d * newGravity + (1 - d) * gravityVector;
+
+    yaw += gravityVector.dot(gyroVector);
+    yaw = fmod(yaw, 2 * PI);
+
+    if (print) {
+      Serial.print(gravityVector(0));
+      Serial.print("\t");
+      Serial.print(gravityVector(1));
+      Serial.print("\t");
+      Serial.print(gravityVector(2));
+      Serial.print("\t-1\t1\t");
+      Serial.print(gyroVector(0));
+      Serial.print("\t");
+      Serial.print(gyroVector(1));
+      Serial.print("\t");
+      Serial.println(gyroVector(2));
+    }
+
+    writeToBLE(blecharacc, gravityVector);
+    writeToBLE(blechargyr, gyroVector);
+  }
+
+  void startBLEadv(void)
+  {
+    Serial.println("[INFO]{BLE} Start advertising...");
+    // Advertising packet
+    Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
+    Bluefruit.Advertising.addTxPower();
+
+    // Include HRM Service UUID
+    Bluefruit.Advertising.addService(bleservice);
+
+    // Secondary Scan Response packet (optional)
+    // Since there is no room for 'Name' in Advertising packet
+    Bluefruit.ScanResponse.addName();
+
+    /* Start Advertising
+    * - Enable auto advertising if disconnected
+    * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
+    * - Timeout for fast mode is 30 seconds
+    * - Start(timeout) with timeout = 0 will advertise forever (until connected)
+    *
+    * For recommended advertising interval
+    * https://developer.apple.com/library/content/qa/qa1931/_index.html
+    */
+    Bluefruit.Advertising.restartOnDisconnect(true);
+    Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
+    Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
+    Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
+  }
+
+}; // end class
+
+Qbead *Qbead::singletoninstance = nullptr;
+
+} // end namespace
+
+#endif // QBEAD_H
\ No newline at end of file

From 105309aaeeff801ee4d1fd15041abcaa3dce3dcb Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Mon, 26 May 2025 15:17:39 +0200
Subject: [PATCH 32/64] added last commit to esp version

---
 src_ESP32/QbeadESP32.h | 11 +++++++++--
 1 file changed, 9 insertions(+), 2 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 333d599..7867637 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -272,8 +272,15 @@ class QuantumState
   void collapse()
   {
     const float theta = stateCoordinates.theta();
-    const float a = cos(theta / 2);
-    const bool is1 = random(0, 100) < a * a * 100;
+    const float a = (cos(theta) + 1) / 2; // probability of measuring |0>
+    if (a < 0.0001) {
+      stateCoordinates.set(0, 0, -1);
+      return;
+    } else if (a > 0.9999) {
+      stateCoordinates.set(0, 0, 1);
+      return;
+    }
+    const bool is1 = random(0, 100) <= a * a * 100;
     this->stateCoordinates.set(0, 0, is1 ? 1 : -1);
   }
 

From 8f8bd14315c9146331b07207fca9016a8c8c6fae Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 26 May 2025 15:23:11 +0200
Subject: [PATCH 33/64] Clean code

---
 src/Qbead.h | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 7d8253a..b13002b 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -20,9 +20,7 @@ using namespace Eigen;
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
-#define GYRO_MEASUREMENT_THRESHOLD 0.0001
-#define GYRO_GATE_THRESHOLD 7
-#define SHAKING_THRESHOLD 0.09
+#define GYRO_GATE_THRESHOLD 8
 #define QB_PIXEL_COUNT 62
 #define QB_MAX_PRPH_CONNECTION 2
 #define T_ACC 100000
@@ -145,7 +143,7 @@ void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
 }
 
 // Tap detection
-LSM6DS3 myIMU(I2C_MODE, 0x6A);
+LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU
 uint8_t interruptCount = 0; // Amount of received interrupts
 uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
 
@@ -384,7 +382,6 @@ class Qbead {
   BLECharacteristic blecharsph;
   BLECharacteristic blecharacc;
   BLECharacteristic blechargyr;
-  uint8_t connection_count = 0;
 
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
@@ -403,7 +400,7 @@ class Qbead {
   uint32_t c_ble = 0xffffff; // color as sent over BLE connection
 
   // led map index to Coordinates
-  // This map is for the first version of the pcb
+  // This map is for the first version of the flex-pcb
   Coordinates led_map_v1[62] = {
     Coordinates(-1, -0, -0),
     Coordinates(-0.866, 0, -0.5),

From 3faeb85520a02744ba9fe787278e0c6ee1d2feb1 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 26 May 2025 15:25:26 +0200
Subject: [PATCH 34/64] Clean code

---
 src_ESP32/QbeadESP32.h | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 7867637..a33d103 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -19,9 +19,7 @@ using namespace Eigen;
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
-#define GYRO_MEASUREMENT_THRESHOLD 0.0001
-#define GYRO_GATE_THRESHOLD 7
-#define SHAKING_THRESHOLD 0.09
+#define GYRO_GATE_THRESHOLD 8
 #define QB_PIXEL_COUNT 62
 #define QB_MAX_PRPH_CONNECTION 2
 #define T_ACC 100000
@@ -144,7 +142,7 @@ void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
 }
 
 // Tap detection
-LSM6DS3 myIMU(I2C_MODE, 0x6A);
+LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU
 uint8_t interruptCount = 0; // Amount of received interrupts
 uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
 
@@ -383,7 +381,6 @@ class Qbead {
   BLECharacteristic blecharsph;
   BLECharacteristic blecharacc;
   BLECharacteristic blechargyr;
-  uint8_t connection_count = 0;
 
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
@@ -402,7 +399,7 @@ class Qbead {
   uint32_t c_ble = 0xffffff; // color as sent over BLE connection
 
   // led map index to Coordinates
-  // This map is for the first version of the pcb
+  // This map is for the first version of the flex-pcb
   Coordinates led_map_v1[62] = {
     Coordinates(-1, -0, -0),
     Coordinates(-0.866, 0, -0.5),

From faab17c748dc668dc3318e9d28ce4fbe8705e1a0 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 26 May 2025 15:49:57 +0200
Subject: [PATCH 35/64] Remove bluetooth from esp 32 version code

---
 examples/IMU_reader/IMU_reader.ino |   1 -
 src_ESP32/QbeadESP32.h             | 138 +----------------------------
 2 files changed, 2 insertions(+), 137 deletions(-)

diff --git a/examples/IMU_reader/IMU_reader.ino b/examples/IMU_reader/IMU_reader.ino
index 9cac340..6d7e745 100644
--- a/examples/IMU_reader/IMU_reader.ino
+++ b/examples/IMU_reader/IMU_reader.ino
@@ -4,7 +4,6 @@ Qbead::Qbead bead;
 
 void setup() {
   bead.begin();
-  bead.startAccelerometer();
   bead.setBrightness(25); // way too bright
   Serial.println("testing all pixels discretely");
   for (int i = 0; i < bead.pixels.numPixels(); i++) {
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index a33d103..2bd7dfd 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -102,18 +102,6 @@ bool checkThetaAndPhi(float theta, float phi) {
   return theta >= 0 && theta <= 180 && phi >= 0 && phi <= 360;
 }
 
-void connect_callback(uint16_t conn_handle)
-{
-  // Get the reference to current connection
-  BLEConnection* connection = Bluefruit.Connection(conn_handle);
-
-  char central_name[32] = { 0 };
-  connection->getPeerName(central_name, sizeof(central_name));
-
-  Serial.print("[INFO]{BLE} Connected to "); // TODO take care of cases where Serial is not available
-  Serial.println(central_name);
-}
-
 // In rads
 void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
 {
@@ -363,25 +351,12 @@ class Qbead {
         const uint16_t pixelconfig = QB_PIXELCONFIG,
         const uint8_t imu_addr = QB_IMU_ADDR)
       : imu(LSM6DS3(I2C_MODE, imu_addr)),
-        pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig)),
-        bleservice(QB_UUID_SERVICE),
-        blecharcol(QB_UUID_COL_CHAR),
-        blecharsph(QB_UUID_SPH_CHAR),
-        blecharacc(QB_UUID_ACC_CHAR),
-        blechargyr(QB_UUID_GYR_CHAR)
+        pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
 
-  static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
-
   LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
-  BLEService bleservice;
-  BLECharacteristic blecharcol;
-  BLECharacteristic blecharsph;
-  BLECharacteristic blecharacc;
-  BLECharacteristic blechargyr;
-
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
@@ -395,9 +370,6 @@ class Qbead {
   Vector3d gyroVector = Vector3d(0, 0, 1);
   float yaw = 0;
 
-  float t_ble, p_ble; // theta and phi as sent over BLE connection
-  uint32_t c_ble = 0xffffff; // color as sent over BLE connection
-
   // led map index to Coordinates
   // This map is for the first version of the flex-pcb
   Coordinates led_map_v1[62] = {
@@ -465,35 +437,7 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
-  static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
-    Serial.println("[INFO]{BLE} Received a write on the color characteristic");
-    singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
-    Serial.print("[DEBUG]{BLE} Received");
-    Serial.println(singletoninstance->c_ble, HEX);
-  }
-
-  static void ble_callback_theta_phi(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len){
-    Serial.println("[INFO]{BLE} Received a write on the spherical coordinates characteristic");
-    singletoninstance->t_ble = ((uint32_t)data[0])*180/255;
-    singletoninstance->p_ble = ((uint32_t)data[1])*360/255;
-    Serial.print("[DEBUG]{BLE} Received t=");
-    Serial.print(singletoninstance->t_ble);
-    Serial.print(" p=");
-    Serial.println(singletoninstance->p_ble);
-  }
-
-  void startAccelerometer() {
-    // BLE Characteristic IMU xyz accelerometer readout
-    blecharacc.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
-    blecharacc.setPermission(SECMODE_OPEN, SECMODE_OPEN);
-    blecharacc.setUserDescriptor("xyz acceleration");
-    blecharacc.setFixedLen(3*sizeof(float));
-    blecharacc.begin();
-    blecharacc.write(zerobuffer20, 3*sizeof(float));    
-  }
-
   void begin() {
-    singletoninstance = this;
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
     {
@@ -505,43 +449,13 @@ class Qbead {
     clear();
     setBrightness(10);
 
-    Serial.println("[INFO] Booting... Qbead on XIAO BLE Sense + LSM6DS3 compiled on " __DATE__ " at " __TIME__);
+    Serial.println("[INFO] Booting... Qbead on XIAO ESP32 compiled on " __DATE__ " at " __TIME__);
     if (!imu.begin()) {
       Serial.println("[DEBUG]{IMU} IMU initialized correctly");
     } else {
       Serial.println("[ERROR]{IMU} IMU failed to initialize");
     }
 
-    // BLE Peripheral service setup
-    Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
-    Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
-    Bluefruit.Periph.setConnectCallback(connect_callback);
-    bleservice.begin();
-    // BLE Characteristic Bloch Sphere Visualizer color setup
-    blecharcol.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
-    blecharcol.setPermission(SECMODE_OPEN, SECMODE_OPEN);
-    blecharcol.setUserDescriptor("BSV rgb color");
-    blecharcol.setFixedLen(3);
-    blecharcol.setWriteCallback(ble_callback_color);
-    blecharcol.begin();
-    blecharcol.write(zerobuffer20, 3);
-    // BLE Characteristic Bloch Sphere Visualizer spherical coordinate setup
-    blecharsph.setProperties(CHR_PROPS_READ | CHR_PROPS_WRITE);
-    blecharsph.setPermission(SECMODE_OPEN, SECMODE_OPEN);
-    blecharsph.setUserDescriptor("BSV spherical coordinates");
-    blecharsph.setFixedLen(2);
-    blecharsph.setWriteCallback(ble_callback_theta_phi);
-    blecharsph.begin();
-    blecharsph.write(zerobuffer20, 2);
-    // BLE Characteristic IMU xyz gyroscope readout
-    blechargyr.setProperties(CHR_PROPS_READ | CHR_PROPS_NOTIFY);
-    blechargyr.setPermission(SECMODE_OPEN, SECMODE_OPEN);
-    blechargyr.setUserDescriptor("xyz gyroscope");
-    blechargyr.setFixedLen(3*sizeof(float));
-    blechargyr.begin();
-    blechargyr.write(zerobuffer20, 3*sizeof(float));
-    startBLEadv();
-
     // Tap detection
     setupTapInterrupt();
     pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
@@ -751,18 +665,6 @@ class Qbead {
     return 0;
   }
 
-  void writeToBLE(BLECharacteristic& destination, Vector3d vector) {
-    float buffer[3] = {(float)vector(0), (float)vector(1), (float)vector(2)};
-    destination.write(buffer, 3 * sizeof(float));
-    for (uint16_t conn_hdl = 0; conn_hdl < QB_MAX_PRPH_CONNECTION; conn_hdl++)
-    {
-      if (Bluefruit.connected(conn_hdl) && destination.notifyEnabled(conn_hdl))
-      {
-        destination.notify(buffer, 3 * sizeof(float));
-      }
-    }
-  }
-
   Vector3d getVectorFromBuffer(float *buffer) {
     // calibration of imu because imu is not aligned with bloch sphere
     float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
@@ -807,44 +709,8 @@ class Qbead {
       Serial.print("\t");
       Serial.println(gyroVector(2));
     }
-
-    writeToBLE(blecharacc, gravityVector);
-    writeToBLE(blechargyr, gyroVector);
-  }
-
-  void startBLEadv(void)
-  {
-    Serial.println("[INFO]{BLE} Start advertising...");
-    // Advertising packet
-    Bluefruit.Advertising.addFlags(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE);
-    Bluefruit.Advertising.addTxPower();
-
-    // Include HRM Service UUID
-    Bluefruit.Advertising.addService(bleservice);
-
-    // Secondary Scan Response packet (optional)
-    // Since there is no room for 'Name' in Advertising packet
-    Bluefruit.ScanResponse.addName();
-
-    /* Start Advertising
-    * - Enable auto advertising if disconnected
-    * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
-    * - Timeout for fast mode is 30 seconds
-    * - Start(timeout) with timeout = 0 will advertise forever (until connected)
-    *
-    * For recommended advertising interval
-    * https://developer.apple.com/library/content/qa/qa1931/_index.html
-    */
-    Bluefruit.Advertising.restartOnDisconnect(true);
-    Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
-    Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
-    Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
   }
-
 }; // end class
-
-Qbead *Qbead::singletoninstance = nullptr;
-
 } // end namespace
 
 #endif // QBEAD_H
\ No newline at end of file

From 28743a11b978015b464100158647e2693ba0a0cc Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 27 May 2025 11:22:08 +0200
Subject: [PATCH 36/64] Generate coordinates map

---
 examples/Decoherence/Decoherence.ino |   7 +-
 generateCoordinates.py               |  46 +++++++++++
 src/Qbead.h                          | 110 ++++++++++++++++++++++++++
 src_ESP32/QbeadESP32.h               | 114 ++++++++++++++++++++++++++-
 4 files changed, 272 insertions(+), 5 deletions(-)
 create mode 100644 generateCoordinates.py

diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino
index 70fcb90..d76db58 100644
--- a/examples/Decoherence/Decoherence.ino
+++ b/examples/Decoherence/Decoherence.ino
@@ -57,11 +57,10 @@ void loop()
         }
     }
     int currentTime = millis();
-    // Increment the amount of leds based on the time since the last collapse
+    // Increment the amount of decoherence based on the time since the last collapse
     // This is to show decoherence
-    int leds = (currentTime - lastCollapseTime) / 2000;
-    leds = constrain(leds, 1, 15);
+    float decoherence = (currentTime - lastCollapseTime) / 2000;
     bead.animateTo(rotationState, 2000);
-    bead.setLed(bead.visualState, stateColor, leds);
+    bead.setLed(bead.visualState, stateColor, decoherence);
     bead.show();
 }
\ No newline at end of file
diff --git a/generateCoordinates.py b/generateCoordinates.py
new file mode 100644
index 0000000..3273a45
--- /dev/null
+++ b/generateCoordinates.py
@@ -0,0 +1,46 @@
+# This is a script to convert the pick and place file to a coordinates map
+# This code is very much not optimized, but it does its job.
+
+import math
+
+inputFile = "../pick-and-place.csv"
+outputFile = "../pick-and-place-coordinates.csv"
+
+lines = []
+
+with open(inputFile, 'r') as file:
+    for line in file:
+        if line.strip():  # Check if the line is not empty
+            lines.append(line.strip())
+
+x0 = float(lines[0].split(',')[3])
+y0 = float(lines[0].split(',')[4])
+
+coordinates = []
+for line in lines:
+    parts = line.split(',')
+    if len(parts) >= 2:
+        x = float(parts[3]) - x0
+        y = float(parts[4]) - y0
+        coordinates.append((x, y))
+
+# Convert to spherical coordinates
+spherical_coordinates = []
+for x, y in coordinates:
+    r = (x**2 + y**2)**0.5
+    phi = math.atan2(y, x)  # angle in radians
+    if phi < 0:
+        phi += 2 * math.pi  # Normalize angle to [0, 2π)
+    spherical_coordinates.append((r, phi))
+
+# scale r from 0 to pi
+scaled_coordinates = []
+for r, phi in spherical_coordinates:
+    scaled_r = r / max(r for r, _ in spherical_coordinates) * math.pi
+    scaled_coordinates.append((round(scaled_r, 2), round(phi, 2)))
+
+# Write to output file
+with open(outputFile, 'w') as file:
+    for theta, phi in scaled_coordinates:
+        file.write(f"Coordinates({theta}, {phi}),\n")
+print(f"Coordinates written to {outputFile}")
diff --git a/src/Qbead.h b/src/Qbead.h
index b13002b..e2013ec 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -466,6 +466,116 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
+  Coordinates led_map_v2[107] = {
+    Coordinates(0.0, 0.0),
+    Coordinates(0.39, 4.97),
+    Coordinates(0.78, 4.97),
+    Coordinates(1.18, 5.08),
+    Coordinates(1.18, 4.87),
+    Coordinates(1.57, 4.89),
+    Coordinates(1.57, 5.06),
+    Coordinates(1.95, 5.04),
+    Coordinates(1.95, 4.91),
+    Coordinates(2.34, 4.97),
+    Coordinates(2.73, 4.97),
+    Coordinates(0.78, 4.45),
+    Coordinates(1.18, 4.55),
+    Coordinates(1.18, 4.35),
+    Coordinates(1.57, 4.37),
+    Coordinates(1.57, 4.53),
+    Coordinates(1.95, 4.51),
+    Coordinates(1.95, 4.39),
+    Coordinates(2.34, 4.45),
+    Coordinates(0.39, 3.93),
+    Coordinates(0.78, 3.93),
+    Coordinates(1.18, 4.03),
+    Coordinates(1.18, 3.82),
+    Coordinates(1.57, 3.84),
+    Coordinates(1.57, 4.01),
+    Coordinates(1.95, 3.99),
+    Coordinates(1.95, 3.87),
+    Coordinates(2.34, 3.93),
+    Coordinates(2.73, 3.93),
+    Coordinates(0.78, 3.4),
+    Coordinates(1.18, 3.3),
+    Coordinates(1.57, 3.32),
+    Coordinates(1.95, 3.34),
+    Coordinates(2.34, 3.4),
+    Coordinates(0.39, 2.88),
+    Coordinates(0.78, 2.88),
+    Coordinates(1.18, 2.98),
+    Coordinates(1.18, 2.78),
+    Coordinates(1.57, 2.8),
+    Coordinates(1.57, 2.96),
+    Coordinates(1.95, 2.94),
+    Coordinates(1.95, 2.82),
+    Coordinates(2.34, 2.88),
+    Coordinates(2.73, 2.88),
+    Coordinates(0.78, 2.36),
+    Coordinates(1.18, 2.46),
+    Coordinates(1.18, 2.25),
+    Coordinates(1.57, 2.27),
+    Coordinates(1.57, 2.44),
+    Coordinates(1.95, 2.42),
+    Coordinates(1.95, 2.29),
+    Coordinates(2.34, 2.36),
+    Coordinates(0.39, 1.83),
+    Coordinates(0.78, 1.83),
+    Coordinates(1.18, 1.93),
+    Coordinates(1.18, 1.73),
+    Coordinates(1.57, 1.75),
+    Coordinates(1.57, 1.92),
+    Coordinates(1.95, 1.89),
+    Coordinates(1.95, 1.77),
+    Coordinates(2.34, 1.83),
+    Coordinates(2.73, 1.83),
+    Coordinates(0.78, 1.31),
+    Coordinates(1.18, 1.41),
+    Coordinates(1.18, 1.21),
+    Coordinates(1.57, 1.23),
+    Coordinates(1.57, 1.39),
+    Coordinates(1.95, 1.37),
+    Coordinates(1.95, 1.25),
+    Coordinates(2.34, 1.31),
+    Coordinates(0.39, 0.79),
+    Coordinates(0.78, 0.79),
+    Coordinates(1.18, 0.89),
+    Coordinates(1.18, 0.68),
+    Coordinates(1.57, 0.7),
+    Coordinates(1.57, 0.87),
+    Coordinates(1.95, 0.85),
+    Coordinates(1.95, 0.72),
+    Coordinates(2.34, 0.79),
+    Coordinates(2.73, 0.79),
+    Coordinates(0.78, 0.26),
+    Coordinates(1.18, 0.36),
+    Coordinates(1.18, 0.16),
+    Coordinates(1.57, 0.18),
+    Coordinates(1.57, 0.35),
+    Coordinates(1.95, 0.32),
+    Coordinates(1.95, 0.2),
+    Coordinates(2.34, 0.26),
+    Coordinates(0.39, 6.02),
+    Coordinates(0.78, 6.02),
+    Coordinates(1.18, 6.12),
+    Coordinates(1.18, 5.92),
+    Coordinates(1.57, 5.94),
+    Coordinates(1.57, 6.1),
+    Coordinates(1.95, 6.08),
+    Coordinates(1.95, 5.96),
+    Coordinates(2.34, 6.02),
+    Coordinates(2.73, 6.02),
+    Coordinates(0.78, 5.5),
+    Coordinates(1.18, 5.6),
+    Coordinates(1.18, 5.4),
+    Coordinates(1.57, 5.41),
+    Coordinates(1.57, 5.58),
+    Coordinates(1.95, 5.56),
+    Coordinates(1.95, 5.44),
+    Coordinates(2.34, 5.5),
+    Coordinates(3.14, 4.97),
+  };
+
   static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
     Serial.println("[INFO]{BLE} Received a write on the color characteristic");
     singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 2bd7dfd..6e4dead 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -437,7 +437,119 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
-  void begin() {
+  Coordinates led_map_v2[107] = {
+    Coordinates(0.0, 0.0),
+    Coordinates(0.39, 4.97),
+    Coordinates(0.78, 4.97),
+    Coordinates(1.18, 5.08),
+    Coordinates(1.18, 4.87),
+    Coordinates(1.57, 4.89),
+    Coordinates(1.57, 5.06),
+    Coordinates(1.95, 5.04),
+    Coordinates(1.95, 4.91),
+    Coordinates(2.34, 4.97),
+    Coordinates(2.73, 4.97),
+    Coordinates(0.78, 4.45),
+    Coordinates(1.18, 4.55),
+    Coordinates(1.18, 4.35),
+    Coordinates(1.57, 4.37),
+    Coordinates(1.57, 4.53),
+    Coordinates(1.95, 4.51),
+    Coordinates(1.95, 4.39),
+    Coordinates(2.34, 4.45),
+    Coordinates(0.39, 3.93),
+    Coordinates(0.78, 3.93),
+    Coordinates(1.18, 4.03),
+    Coordinates(1.18, 3.82),
+    Coordinates(1.57, 3.84),
+    Coordinates(1.57, 4.01),
+    Coordinates(1.95, 3.99),
+    Coordinates(1.95, 3.87),
+    Coordinates(2.34, 3.93),
+    Coordinates(2.73, 3.93),
+    Coordinates(0.78, 3.4),
+    Coordinates(1.18, 3.3),
+    Coordinates(1.57, 3.32),
+    Coordinates(1.95, 3.34),
+    Coordinates(2.34, 3.4),
+    Coordinates(0.39, 2.88),
+    Coordinates(0.78, 2.88),
+    Coordinates(1.18, 2.98),
+    Coordinates(1.18, 2.78),
+    Coordinates(1.57, 2.8),
+    Coordinates(1.57, 2.96),
+    Coordinates(1.95, 2.94),
+    Coordinates(1.95, 2.82),
+    Coordinates(2.34, 2.88),
+    Coordinates(2.73, 2.88),
+    Coordinates(0.78, 2.36),
+    Coordinates(1.18, 2.46),
+    Coordinates(1.18, 2.25),
+    Coordinates(1.57, 2.27),
+    Coordinates(1.57, 2.44),
+    Coordinates(1.95, 2.42),
+    Coordinates(1.95, 2.29),
+    Coordinates(2.34, 2.36),
+    Coordinates(0.39, 1.83),
+    Coordinates(0.78, 1.83),
+    Coordinates(1.18, 1.93),
+    Coordinates(1.18, 1.73),
+    Coordinates(1.57, 1.75),
+    Coordinates(1.57, 1.92),
+    Coordinates(1.95, 1.89),
+    Coordinates(1.95, 1.77),
+    Coordinates(2.34, 1.83),
+    Coordinates(2.73, 1.83),
+    Coordinates(0.78, 1.31),
+    Coordinates(1.18, 1.41),
+    Coordinates(1.18, 1.21),
+    Coordinates(1.57, 1.23),
+    Coordinates(1.57, 1.39),
+    Coordinates(1.95, 1.37),
+    Coordinates(1.95, 1.25),
+    Coordinates(2.34, 1.31),
+    Coordinates(0.39, 0.79),
+    Coordinates(0.78, 0.79),
+    Coordinates(1.18, 0.89),
+    Coordinates(1.18, 0.68),
+    Coordinates(1.57, 0.7),
+    Coordinates(1.57, 0.87),
+    Coordinates(1.95, 0.85),
+    Coordinates(1.95, 0.72),
+    Coordinates(2.34, 0.79),
+    Coordinates(2.73, 0.79),
+    Coordinates(0.78, 0.26),
+    Coordinates(1.18, 0.36),
+    Coordinates(1.18, 0.16),
+    Coordinates(1.57, 0.18),
+    Coordinates(1.57, 0.35),
+    Coordinates(1.95, 0.32),
+    Coordinates(1.95, 0.2),
+    Coordinates(2.34, 0.26),
+    Coordinates(0.39, 6.02),
+    Coordinates(0.78, 6.02),
+    Coordinates(1.18, 6.12),
+    Coordinates(1.18, 5.92),
+    Coordinates(1.57, 5.94),
+    Coordinates(1.57, 6.1),
+    Coordinates(1.95, 6.08),
+    Coordinates(1.95, 5.96),
+    Coordinates(2.34, 6.02),
+    Coordinates(2.73, 6.02),
+    Coordinates(0.78, 5.5),
+    Coordinates(1.18, 5.6),
+    Coordinates(1.18, 5.4),
+    Coordinates(1.57, 5.41),
+    Coordinates(1.57, 5.58),
+    Coordinates(1.95, 5.56),
+    Coordinates(1.95, 5.44),
+    Coordinates(2.34, 5.5),
+    Coordinates(3.14, 4.97),
+  };
+
+  void
+  begin()
+  {
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
     {

From 01d9e9ff399c24cd3a7ecb12d5d39b9232a47acc Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 27 May 2025 11:24:24 +0200
Subject: [PATCH 37/64] Add todo

---
 src/Qbead.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/Qbead.h b/src/Qbead.h
index e2013ec..34ed37c 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -466,6 +466,7 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
+  // TODO: check when the flex-pcb is in production
   Coordinates led_map_v2[107] = {
     Coordinates(0.0, 0.0),
     Coordinates(0.39, 4.97),

From 1ba78a624c3a09f99df730c1e566f16aee9d9cf2 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 27 May 2025 11:25:22 +0200
Subject: [PATCH 38/64] Add todo

---
 src/Qbead.h            | 111 -----------------------------------------
 src_ESP32/QbeadESP32.h |   1 +
 2 files changed, 1 insertion(+), 111 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 34ed37c..b13002b 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -466,117 +466,6 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
-  // TODO: check when the flex-pcb is in production
-  Coordinates led_map_v2[107] = {
-    Coordinates(0.0, 0.0),
-    Coordinates(0.39, 4.97),
-    Coordinates(0.78, 4.97),
-    Coordinates(1.18, 5.08),
-    Coordinates(1.18, 4.87),
-    Coordinates(1.57, 4.89),
-    Coordinates(1.57, 5.06),
-    Coordinates(1.95, 5.04),
-    Coordinates(1.95, 4.91),
-    Coordinates(2.34, 4.97),
-    Coordinates(2.73, 4.97),
-    Coordinates(0.78, 4.45),
-    Coordinates(1.18, 4.55),
-    Coordinates(1.18, 4.35),
-    Coordinates(1.57, 4.37),
-    Coordinates(1.57, 4.53),
-    Coordinates(1.95, 4.51),
-    Coordinates(1.95, 4.39),
-    Coordinates(2.34, 4.45),
-    Coordinates(0.39, 3.93),
-    Coordinates(0.78, 3.93),
-    Coordinates(1.18, 4.03),
-    Coordinates(1.18, 3.82),
-    Coordinates(1.57, 3.84),
-    Coordinates(1.57, 4.01),
-    Coordinates(1.95, 3.99),
-    Coordinates(1.95, 3.87),
-    Coordinates(2.34, 3.93),
-    Coordinates(2.73, 3.93),
-    Coordinates(0.78, 3.4),
-    Coordinates(1.18, 3.3),
-    Coordinates(1.57, 3.32),
-    Coordinates(1.95, 3.34),
-    Coordinates(2.34, 3.4),
-    Coordinates(0.39, 2.88),
-    Coordinates(0.78, 2.88),
-    Coordinates(1.18, 2.98),
-    Coordinates(1.18, 2.78),
-    Coordinates(1.57, 2.8),
-    Coordinates(1.57, 2.96),
-    Coordinates(1.95, 2.94),
-    Coordinates(1.95, 2.82),
-    Coordinates(2.34, 2.88),
-    Coordinates(2.73, 2.88),
-    Coordinates(0.78, 2.36),
-    Coordinates(1.18, 2.46),
-    Coordinates(1.18, 2.25),
-    Coordinates(1.57, 2.27),
-    Coordinates(1.57, 2.44),
-    Coordinates(1.95, 2.42),
-    Coordinates(1.95, 2.29),
-    Coordinates(2.34, 2.36),
-    Coordinates(0.39, 1.83),
-    Coordinates(0.78, 1.83),
-    Coordinates(1.18, 1.93),
-    Coordinates(1.18, 1.73),
-    Coordinates(1.57, 1.75),
-    Coordinates(1.57, 1.92),
-    Coordinates(1.95, 1.89),
-    Coordinates(1.95, 1.77),
-    Coordinates(2.34, 1.83),
-    Coordinates(2.73, 1.83),
-    Coordinates(0.78, 1.31),
-    Coordinates(1.18, 1.41),
-    Coordinates(1.18, 1.21),
-    Coordinates(1.57, 1.23),
-    Coordinates(1.57, 1.39),
-    Coordinates(1.95, 1.37),
-    Coordinates(1.95, 1.25),
-    Coordinates(2.34, 1.31),
-    Coordinates(0.39, 0.79),
-    Coordinates(0.78, 0.79),
-    Coordinates(1.18, 0.89),
-    Coordinates(1.18, 0.68),
-    Coordinates(1.57, 0.7),
-    Coordinates(1.57, 0.87),
-    Coordinates(1.95, 0.85),
-    Coordinates(1.95, 0.72),
-    Coordinates(2.34, 0.79),
-    Coordinates(2.73, 0.79),
-    Coordinates(0.78, 0.26),
-    Coordinates(1.18, 0.36),
-    Coordinates(1.18, 0.16),
-    Coordinates(1.57, 0.18),
-    Coordinates(1.57, 0.35),
-    Coordinates(1.95, 0.32),
-    Coordinates(1.95, 0.2),
-    Coordinates(2.34, 0.26),
-    Coordinates(0.39, 6.02),
-    Coordinates(0.78, 6.02),
-    Coordinates(1.18, 6.12),
-    Coordinates(1.18, 5.92),
-    Coordinates(1.57, 5.94),
-    Coordinates(1.57, 6.1),
-    Coordinates(1.95, 6.08),
-    Coordinates(1.95, 5.96),
-    Coordinates(2.34, 6.02),
-    Coordinates(2.73, 6.02),
-    Coordinates(0.78, 5.5),
-    Coordinates(1.18, 5.6),
-    Coordinates(1.18, 5.4),
-    Coordinates(1.57, 5.41),
-    Coordinates(1.57, 5.58),
-    Coordinates(1.95, 5.56),
-    Coordinates(1.95, 5.44),
-    Coordinates(2.34, 5.5),
-    Coordinates(3.14, 4.97),
-  };
-
   static void ble_callback_color(uint16_t conn_hdl, BLECharacteristic* chr, uint8_t* data, uint16_t len) {
     Serial.println("[INFO]{BLE} Received a write on the color characteristic");
     singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 6e4dead..1909a73 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -437,6 +437,7 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
+  // TODO: Check when the new flex-pcb has arrived
   Coordinates led_map_v2[107] = {
     Coordinates(0.0, 0.0),
     Coordinates(0.39, 4.97),

From 55debe5646eac491a90adba41de9e8e7dba638ca Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 27 May 2025 11:50:11 +0200
Subject: [PATCH 39/64] Code can now be uploaded to the esp32

---
 src_ESP32/QbeadESP32.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 1909a73..11d958f 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -570,9 +570,9 @@ class Qbead {
     }
 
     // Tap detection
-    setupTapInterrupt();
-    pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
-    attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
+    // setupTapInterrupt();
+    // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
+    // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
   }
 
   void clear() {

From 92cfed0d49223c202645ab96bf9d82617239973c Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 27 May 2025 14:04:46 +0200
Subject: [PATCH 40/64] Reduce decoherence to 1 axis and implement dynamic
 decoupling

---
 examples/Decoherence/Decoherence.ino | 33 ++++++++++++++++++++--------
 src/Qbead.h                          |  2 +-
 src_ESP32/QbeadESP32.h               |  2 +-
 3 files changed, 26 insertions(+), 11 deletions(-)

diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino
index d76db58..61dc9f5 100644
--- a/examples/Decoherence/Decoherence.ino
+++ b/examples/Decoherence/Decoherence.ino
@@ -3,8 +3,11 @@
 Qbead::Qbead bead;
 int rotationState = 0;
 uint32_t stateColor = color(255, 255, 255);
+uint32_t decoherenceColor = color(122, 0, 122);
 const bool toggleAnimationOn = 1;
-int lastCollapseTime = 0;
+Qbead::Coordinates oldCoordinates(0, 0, 1);
+int t = 0;
+bool wasFrozen = false;
 
 void setup()
 {
@@ -28,7 +31,8 @@ void setup()
         }
     }
     Serial.println("starting inertial tracking");
-    lastCollapseTime = millis();
+    oldCoordinates = bead.state.getCoordinates();
+    t = millis();
 }
 
 void loop()
@@ -42,25 +46,36 @@ void loop()
     if (bead.frozen)
     {
         stateColor = color(122, 122, 0);
+        wasFrozen = true;
     }
     else
     {
+        if (wasFrozen)
+        {
+            wasFrozen = false;
+            oldCoordinates = bead.state.getCoordinates();
+        }
         rotationState = bead.checkMotion();
         if (rotationState != 0)
         {
             bead.frozen = true;
             bead.T_freeze = millis();
         }
-        if (rotationState == 8)
+        float phi = bead.state.getCoordinates().phi();
+        int dt = millis() - t;
+        float randInt = random(200, 10000);
+        phi += dt / randInt;
+        if (phi > 2 * PI)
         {
-            lastCollapseTime = millis();
+            phi -= 2 * PI;
         }
+        Qbead::Coordinates newCoordinates(bead.state.getCoordinates().theta(), phi);
+        bead.state.setCoordinates(newCoordinates);
+        bead.visualState = bead.state.getCoordinates();
+        bead.setLed(oldCoordinates, decoherenceColor);
     }
-    int currentTime = millis();
-    // Increment the amount of decoherence based on the time since the last collapse
-    // This is to show decoherence
-    float decoherence = (currentTime - lastCollapseTime) / 2000;
+    t = millis();
     bead.animateTo(rotationState, 2000);
-    bead.setLed(bead.visualState, stateColor, decoherence);
+    bead.setLed(bead.visualState, stateColor, 1);
     bead.show();
 }
\ No newline at end of file
diff --git a/src/Qbead.h b/src/Qbead.h
index b13002b..3b9bdf4 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -671,7 +671,7 @@ class Qbead {
       }
       if (totalAcceleration > 11)
       {
-        Serial.println("RANDOMISING");
+        Serial.println("Randomizing");
         float randomTheta = (random(0, 1000)/1000.0f) * PI;
         float randomPhi = (random(0, 1000)/500.0f) * PI;
         state.setCoordinates(Coordinates(randomTheta, randomPhi));
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 11d958f..6164c12 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -697,7 +697,7 @@ class Qbead {
       }
       if (totalAcceleration > 11)
       {
-        Serial.println("RANDOMISING");
+        Serial.println("Randomizing");
         float randomTheta = (random(0, 1000)/1000.0f) * PI;
         float randomPhi = (random(0, 1000)/500.0f) * PI;
         state.setCoordinates(Coordinates(randomTheta, randomPhi));

From db30d1dd884f95b82c666cd6247b1eda84c160d2 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 27 May 2025 14:41:25 +0200
Subject: [PATCH 41/64] added simple esp32 BLE

---
 .../PauliGate_detection.ino                   |   2 +-
 src_ESP32/QbeadESP32.h                        | 101 ++++++++++++++++--
 2 files changed, 93 insertions(+), 10 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index c8dc102..fa6ccf5 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -1,4 +1,4 @@
-#include <Qbead.h>
+#include <QbeadESP32.h>
 
 Qbead::Qbead bead;
 int rotationState = 0;
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 2bd7dfd..7a760b9 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -6,6 +6,9 @@
 #include <LSM6DS3.h>
 #include <math.h>
 #include <ArduinoEigen.h>
+#include <BLEDevice.h>
+#include <BLEUtils.h>
+#include <BLEServer.h>
 
 using namespace Eigen;
 
@@ -25,15 +28,15 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
-const uint8_t QB_UUID_SERVICE[] =
+const char QB_UUID_SERVICE[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_COL_CHAR[] =
+const char QB_UUID_COL_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_SPH_CHAR[] =
+const char QB_UUID_SPH_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_ACC_CHAR[] =
+const char QB_UUID_ACC_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_GYR_CHAR[] =
+const char QB_UUID_GYR_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
 
 const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
@@ -357,6 +360,14 @@ class Qbead {
   LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
+  BLEServer* bleserver;
+  BLEService* bleservice;
+  BLECharacteristic* blecharcol;
+  BLECharacteristic* blecharsph;
+  BLECharacteristic* blecharacc;
+  BLECharacteristic* blechargyr;
+  BLEAdvertising* bleadvertising;
+
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
@@ -437,6 +448,12 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
+  void startAccelerometer() 
+  {
+    blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+  }
+
   void begin() {
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
@@ -456,10 +473,67 @@ class Qbead {
       Serial.println("[ERROR]{IMU} IMU failed to initialize");
     }
 
+    BLEDevice::init("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
+    // Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.Periph.setConnectCallback(connect_callback);
+    bleserver = BLEDevice::createServer();
+    bleservice = bleserver->createService(QB_UUID_SERVICE);
+    // BLE Characteristic Bloch Sphere Visualizer color setup
+
+    uint8_t zerobuffer2[] = {0 ,0};
+    float zerobufferfloat[] = {0.0f, 0.0f, 0.0f};
+    blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    
+    blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    
+    
+    blechargyr = bleservice->createCharacteristic(QB_UUID_GYR_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+
+    bleservice->start();
+    startBLEadv();
     // Tap detection
-    setupTapInterrupt();
-    pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
-    attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
+    // setupTapInterrupt();
+    // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
+    // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
+  }
+
+  void startBLEadv(void)
+  {
+    bleadvertising = bleserver->getAdvertising();
+    bleadvertising->addServiceUUID(QB_UUID_SERVICE);
+    Serial.println("[INFO]{BLE} Start advertising...");
+    // Advertising packet
+    BLEAdvertisementData advertisementData;
+    advertisementData.setFlags(6); // BLE_SIG_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = 6
+
+    // Bluefruit.Advertising.addTxPower();
+
+    // Secondary Scan Response packet (optional)
+    // Since there is no room for 'Name' in Advertising packet
+    // Bluefruit.ScanResponse.addName();
+
+    /* Start Advertising
+    * - Enable auto advertising if disconnected
+    * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
+    * - Timeout for fast mode is 30 seconds
+    * - Start(timeout) with timeout = 0 will advertise forever (until connected)
+    *
+    * For recommended advertising interval
+    * https://developer.apple.com/library/content/qa/qa1931/_index.html
+    */
+    bleadvertising->setAdvertisementData(advertisementData);
+    // Bluefruit.Advertising.restartOnDisconnect(true);
+    bleadvertising->setMinInterval(32);
+    bleadvertising->setMaxInterval(244);
+  
+    BLEDevice::startAdvertising();
+    // Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
+    // Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
+    // Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
   }
 
   void clear() {
@@ -665,6 +739,12 @@ class Qbead {
     return 0;
   }
 
+  void writeToBLE(BLECharacteristic* destination, Vector3d vector) {
+    float buffer[] = {(float)vector(0), (float)vector(1), (float)vector(2)};
+    destination->setValue((uint8_t*)buffer, sizeof(buffer));
+    destination->notify();
+  }
+
   Vector3d getVectorFromBuffer(float *buffer) {
     // calibration of imu because imu is not aligned with bloch sphere
     float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
@@ -709,8 +789,11 @@ class Qbead {
       Serial.print("\t");
       Serial.println(gyroVector(2));
     }
+    
+    writeToBLE(blecharacc, gravityVector);
+    writeToBLE(blechargyr, gyroVector);
   }
 }; // end class
 } // end namespace
 
-#endif // QBEAD_H
\ No newline at end of file
+#endif // QBEAD_H 
\ No newline at end of file

From e422c5ee61506cca556ae20f8df6dd43f66f40d2 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 27 May 2025 14:41:25 +0200
Subject: [PATCH 42/64] added simple esp32 BLE

---
 .../PauliGate_detection.ino                   |   2 +-
 src_ESP32/QbeadESP32.h                        | 101 ++++++++++++++++--
 2 files changed, 96 insertions(+), 7 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index c8dc102..fa6ccf5 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -1,4 +1,4 @@
-#include <Qbead.h>
+#include <QbeadESP32.h>
 
 Qbead::Qbead bead;
 int rotationState = 0;
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 6164c12..e44e886 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -6,6 +6,9 @@
 #include <LSM6DS3.h>
 #include <math.h>
 #include <ArduinoEigen.h>
+#include <BLEDevice.h>
+#include <BLEUtils.h>
+#include <BLEServer.h>
 
 using namespace Eigen;
 
@@ -25,15 +28,15 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
-const uint8_t QB_UUID_SERVICE[] =
+const char QB_UUID_SERVICE[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_COL_CHAR[] =
+const char QB_UUID_COL_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_SPH_CHAR[] =
+const char QB_UUID_SPH_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_ACC_CHAR[] =
+const char QB_UUID_ACC_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
-const uint8_t QB_UUID_GYR_CHAR[] =
+const char QB_UUID_GYR_CHAR[] =
 {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
 
 const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
@@ -357,6 +360,14 @@ class Qbead {
   LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
+  BLEServer* bleserver;
+  BLEService* bleservice;
+  BLECharacteristic* blecharcol;
+  BLECharacteristic* blecharsph;
+  BLECharacteristic* blecharacc;
+  BLECharacteristic* blechargyr;
+  BLEAdvertising* bleadvertising;
+
   float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
@@ -437,6 +448,18 @@ class Qbead {
     Coordinates(0.866, -0.25, -0.433),
   };
 
+  void startAccelerometer() 
+  {
+    blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+  }
+
+  void startAccelerometer() 
+  {
+    blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+  }
+
   // TODO: Check when the new flex-pcb has arrived
   Coordinates led_map_v2[107] = {
     Coordinates(0.0, 0.0),
@@ -569,12 +592,69 @@ class Qbead {
       Serial.println("[ERROR]{IMU} IMU failed to initialize");
     }
 
+    BLEDevice::init("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
+    // Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.Periph.setConnectCallback(connect_callback);
+    bleserver = BLEDevice::createServer();
+    bleservice = bleserver->createService(QB_UUID_SERVICE);
+    // BLE Characteristic Bloch Sphere Visualizer color setup
+
+    uint8_t zerobuffer2[] = {0 ,0};
+    float zerobufferfloat[] = {0.0f, 0.0f, 0.0f};
+    blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    
+    blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    
+    
+    blechargyr = bleservice->createCharacteristic(QB_UUID_GYR_CHAR,
+                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+
+    bleservice->start();
+    startBLEadv();
     // Tap detection
     // setupTapInterrupt();
     // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
     // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
   }
 
+  void startBLEadv(void)
+  {
+    bleadvertising = bleserver->getAdvertising();
+    bleadvertising->addServiceUUID(QB_UUID_SERVICE);
+    Serial.println("[INFO]{BLE} Start advertising...");
+    // Advertising packet
+    BLEAdvertisementData advertisementData;
+    advertisementData.setFlags(6); // BLE_SIG_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = 6
+
+    // Bluefruit.Advertising.addTxPower();
+
+    // Secondary Scan Response packet (optional)
+    // Since there is no room for 'Name' in Advertising packet
+    // Bluefruit.ScanResponse.addName();
+
+    /* Start Advertising
+    * - Enable auto advertising if disconnected
+    * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
+    * - Timeout for fast mode is 30 seconds
+    * - Start(timeout) with timeout = 0 will advertise forever (until connected)
+    *
+    * For recommended advertising interval
+    * https://developer.apple.com/library/content/qa/qa1931/_index.html
+    */
+    bleadvertising->setAdvertisementData(advertisementData);
+    // Bluefruit.Advertising.restartOnDisconnect(true);
+    bleadvertising->setMinInterval(32);
+    bleadvertising->setMaxInterval(244);
+  
+    BLEDevice::startAdvertising();
+    // Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
+    // Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
+    // Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
+  }
+
   void clear() {
     pixels.clear();
   }
@@ -778,6 +858,12 @@ class Qbead {
     return 0;
   }
 
+  void writeToBLE(BLECharacteristic* destination, Vector3d vector) {
+    float buffer[] = {(float)vector(0), (float)vector(1), (float)vector(2)};
+    destination->setValue((uint8_t*)buffer, sizeof(buffer));
+    destination->notify();
+  }
+
   Vector3d getVectorFromBuffer(float *buffer) {
     // calibration of imu because imu is not aligned with bloch sphere
     float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
@@ -822,8 +908,11 @@ class Qbead {
       Serial.print("\t");
       Serial.println(gyroVector(2));
     }
+    
+    writeToBLE(blecharacc, gravityVector);
+    writeToBLE(blechargyr, gyroVector);
   }
 }; // end class
 } // end namespace
 
-#endif // QBEAD_H
\ No newline at end of file
+#endif // QBEAD_H 
\ No newline at end of file

From 54ddb2d2b7fdf03cad7297d0c7fc52a5a7d670c6 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Wed, 28 May 2025 14:01:58 +0200
Subject: [PATCH 43/64] Remove all i2c

---
 src_ESP32/QbeadESP32.h | 48 ++++++++++++++++++++++++------------------
 1 file changed, 27 insertions(+), 21 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 6164c12..c5a504d 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -130,7 +130,7 @@ void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
 }
 
 // Tap detection
-LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU
+// LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU
 uint8_t interruptCount = 0; // Amount of received interrupts
 uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
 
@@ -139,12 +139,12 @@ void setupTapInterrupt() {
   uint8_t dataToWrite = 0;
 
   // Double Tap Config
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C);
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
-  myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C);
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
+  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
 }
 
 void int1ISR()
@@ -350,11 +350,11 @@ class Qbead {
   Qbead(const uint16_t pin00 = QB_LEDPIN,
         const uint16_t pixelconfig = QB_PIXELCONFIG,
         const uint8_t imu_addr = QB_IMU_ADDR)
-      : imu(LSM6DS3(I2C_MODE, imu_addr)),
+      :
         pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
 
-  LSM6DS3 imu;
+  // LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
   float rbuffer[3], rgyrobuffer[3];
@@ -563,11 +563,11 @@ class Qbead {
     setBrightness(10);
 
     Serial.println("[INFO] Booting... Qbead on XIAO ESP32 compiled on " __DATE__ " at " __TIME__);
-    if (!imu.begin()) {
-      Serial.println("[DEBUG]{IMU} IMU initialized correctly");
-    } else {
-      Serial.println("[ERROR]{IMU} IMU failed to initialize");
-    }
+    // if (!imu.begin()) {
+    //   Serial.println("[DEBUG]{IMU} IMU initialized correctly");
+    // } else {
+    //   Serial.println("[ERROR]{IMU} IMU failed to initialize");
+    // }
 
     // Tap detection
     // setupTapInterrupt();
@@ -745,7 +745,7 @@ class Qbead {
     if (interruptCount > prevInterruptCount)
     {
       uint8_t tapStatus = 0;
-      myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
+      // myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
       prevInterruptCount = interruptCount;
 
       if (tapStatus & 0x01)
@@ -787,12 +787,18 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    rbuffer[0] = imu.readFloatAccelX();
-    rbuffer[1] = imu.readFloatAccelY();
-    rbuffer[2] = imu.readFloatAccelZ();    
-    rgyrobuffer[0] = imu.readFloatGyroX();
-    rgyrobuffer[1] = imu.readFloatGyroY();
-    rgyrobuffer[2] = imu.readFloatGyroZ();
+    // rbuffer[0] = imu.readFloatAccelX();
+    // rbuffer[1] = imu.readFloatAccelY();
+    // rbuffer[2] = imu.readFloatAccelZ();    
+    // rgyrobuffer[0] = imu.readFloatGyroX();
+    // rgyrobuffer[1] = imu.readFloatGyroY();
+    // rgyrobuffer[2] = imu.readFloatGyroZ();
+    rbuffer[0] = 0;
+    rbuffer[1] = 0;
+    rbuffer[2] = 1; // gravity vector
+    rgyrobuffer[0] = 0;
+    rgyrobuffer[1] = 0;
+    rgyrobuffer[2] = 0; // gyro vector
 
     float T_new = micros();
     float delta = T_new - T_imu;

From 8efa27b46fc6fcad7e2bd896af01ecd19936fd85 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Wed, 28 May 2025 14:46:39 +0200
Subject: [PATCH 44/64] Use correct pin

---
 src_ESP32/QbeadESP32.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index c5a504d..d9bdbb4 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -10,7 +10,7 @@
 using namespace Eigen;
 
 // default configs
-#define QB_LEDPIN 10
+#define QB_LEDPIN 9
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
 #define QB_IMU_ADDR 0x6A
 #define QB_IX 1

From 6bbfa58abb5e8b69469dea390cd3a116b5397010 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 27 May 2025 14:41:25 +0200
Subject: [PATCH 45/64] added simple esp32 BLE

---
 src_ESP32/QbeadESP32.h | 6 ------
 1 file changed, 6 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index e44e886..48d6f6b 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -454,12 +454,6 @@ class Qbead {
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
   }
 
-  void startAccelerometer() 
-  {
-    blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
-                  BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
-  }
-
   // TODO: Check when the new flex-pcb has arrived
   Coordinates led_map_v2[107] = {
     Coordinates(0.0, 0.0),

From 36dd75aae978832812a496254aefcf10ada13bb0 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Wed, 28 May 2025 15:14:06 +0200
Subject: [PATCH 46/64] WIP BLE ESP32

---
 src_ESP32/QbeadESP32.h | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 48d6f6b..0a2a9c7 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -157,6 +157,25 @@ void int1ISR()
 
 namespace Qbead {
 
+class MyServerCallbacks: public BLEServerCallbacks {
+  void onConnect(BLEServer* pServer) {
+    Serial.println("BLE: Device connected");
+  }
+  void onDisconnect(BLEServer* pServer) {
+    Serial.println("BLE: Device disconnected");
+  }
+};
+
+class ColorCharCallbacks: public BLECharacteristicCallbacks {
+  void onWrite(BLECharacteristic *pCharacteristic) {
+    Serial.println("[INFO]{BLE} Received a write on the color characteristic");
+    uint8_t data[3] = pCharacteristic->getData();
+    Qbead::singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
+    Serial.print("[DEBUG]{BLE} Received");
+    Serial.println(Qbead::singletoninstance->c_ble, HEX);
+  }
+};
+
 class Coordinates
 {
 public:
@@ -356,6 +375,8 @@ class Qbead {
       : imu(LSM6DS3(I2C_MODE, imu_addr)),
         pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
+  
+  static Qbead *singletoninstance;
 
   LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
@@ -373,6 +394,7 @@ class Qbead {
   float T_freeze = 0;
   float T_shaking = 0;
   float shakingCounter = 0;
+  uint32_t = c_ble;
   bool frozen = false; // frozen means that there is an animation in progress
   bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
@@ -568,6 +590,7 @@ class Qbead {
   void
   begin()
   {
+    singletoninstance = this;
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
     {
@@ -591,6 +614,7 @@ class Qbead {
     // Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
     // Bluefruit.Periph.setConnectCallback(connect_callback);
     bleserver = BLEDevice::createServer();
+    bleserver->setCallbacks(new MyServerCallbacks());
     bleservice = bleserver->createService(QB_UUID_SERVICE);
     // BLE Characteristic Bloch Sphere Visualizer color setup
 
@@ -598,6 +622,7 @@ class Qbead {
     float zerobufferfloat[] = {0.0f, 0.0f, 0.0f};
     blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    blecharcol->setCallbacks(new ColorCharCallbacks());
     
     blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
@@ -621,6 +646,7 @@ class Qbead {
     Serial.println("[INFO]{BLE} Start advertising...");
     // Advertising packet
     BLEAdvertisementData advertisementData;
+    advertisementData.setName("qbead | " __DATE__ " " __TIME__);
     advertisementData.setFlags(6); // BLE_SIG_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = 6
 
     // Bluefruit.Advertising.addTxPower();
@@ -907,6 +933,9 @@ class Qbead {
     writeToBLE(blechargyr, gyroVector);
   }
 }; // end class
+
+Qbead *Qbead::singletoninstance = nullptr;
+
 } // end namespace
 
 #endif // QBEAD_H 
\ No newline at end of file

From c2775edabcdce5ad272ed49ad958bcc313aa3e88 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Wed, 28 May 2025 15:49:50 +0200
Subject: [PATCH 47/64] BLE code compiled(But doesn't run properly)

---
 src_ESP32/QbeadESP32.h | 40 ++++++++++++++++++++--------------------
 1 file changed, 20 insertions(+), 20 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index cd6fbe0..c1abd09 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -157,25 +157,6 @@ void int1ISR()
 
 namespace Qbead {
 
-class MyServerCallbacks: public BLEServerCallbacks {
-  void onConnect(BLEServer* pServer) {
-    Serial.println("BLE: Device connected");
-  }
-  void onDisconnect(BLEServer* pServer) {
-    Serial.println("BLE: Device disconnected");
-  }
-};
-
-class ColorCharCallbacks: public BLECharacteristicCallbacks {
-  void onWrite(BLECharacteristic *pCharacteristic) {
-    Serial.println("[INFO]{BLE} Received a write on the color characteristic");
-    uint8_t data[3] = pCharacteristic->getData();
-    Qbead::singletoninstance->c_ble =  (data[2] << 16) | (data[1] << 8) | data[0];
-    Serial.print("[DEBUG]{BLE} Received");
-    Serial.println(Qbead::singletoninstance->c_ble, HEX);
-  }
-};
-
 class Coordinates
 {
 public:
@@ -394,7 +375,7 @@ class Qbead {
   float T_freeze = 0;
   float T_shaking = 0;
   float shakingCounter = 0;
-  uint32_t = c_ble;
+  uint32_t c_ble;
   bool frozen = false; // frozen means that there is an animation in progress
   bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
@@ -587,6 +568,25 @@ class Qbead {
     Coordinates(3.14, 4.97),
   };
 
+  class MyServerCallbacks: public BLEServerCallbacks {
+    void onConnect(BLEServer* pServer) {
+      Serial.println("BLE: Device connected");
+    }
+    void onDisconnect(BLEServer* pServer) {
+      Serial.println("BLE: Device disconnected");
+    }
+  };
+
+  class ColorCharCallbacks: public BLECharacteristicCallbacks {
+    void onWrite(BLECharacteristic *pCharacteristic) {
+      Serial.println("[INFO]{BLE} Received a write on the color characteristic");
+      uint8_t* pData = pCharacteristic->getData();
+      Qbead::singletoninstance->c_ble =  (pData[2] << 16) | (pData[1] << 8) | pData[0];
+      Serial.print("[DEBUG]{BLE} Received");
+      Serial.println(Qbead::singletoninstance->c_ble, HEX);
+    }
+  };
+
   void
   begin()
   {

From 02a2876cfe50d4224172692bfd2e2da791770203 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 2 Jun 2025 13:56:37 +0200
Subject: [PATCH 48/64] Simplify collapsing

---
 src/Qbead.h            | 3 +--
 src_ESP32/QbeadESP32.h | 3 +--
 2 files changed, 2 insertions(+), 4 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index 3b9bdf4..d294037 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -270,8 +270,7 @@ class QuantumState
 
   void collapse()
   {
-    const float theta = stateCoordinates.theta();
-    const float a = (cos(theta) + 1) / 2; // probability of measuring |0>
+    const float a = (stateCoordinates.z() + 1) / 2; // probability of measuring |0>
     if (a < 0.0001) {
       stateCoordinates.set(0, 0, -1);
       return;
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index d9bdbb4..6446523 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -257,8 +257,7 @@ class QuantumState
 
   void collapse()
   {
-    const float theta = stateCoordinates.theta();
-    const float a = (cos(theta) + 1) / 2; // probability of measuring |0>
+    const float a = (stateCoordinates.z() + 1) / 2; // probability of measuring |0>
     if (a < 0.0001) {
       stateCoordinates.set(0, 0, -1);
       return;

From 19f1eae30de44e912ea7f8ea03c10697c59fe147 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 3 Jun 2025 14:28:58 +0200
Subject: [PATCH 49/64] working BLE on esp32

---
 src_ESP32/QbeadESP32.h | 81 ++++++++++++++++++++++++++++++------------
 1 file changed, 58 insertions(+), 23 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index c1abd09..040d72a 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -28,18 +28,18 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
-const char QB_UUID_SERVICE[] =
-{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
-const char QB_UUID_COL_CHAR[] =
-{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3};
-const char QB_UUID_SPH_CHAR[] =
-{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
-const char QB_UUID_ACC_CHAR[] =
-{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
-const char QB_UUID_GYR_CHAR[] =
-{0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
-
-const uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
+// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
+const char QB_UUID_COL_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68c043";
+// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+1,0x1f,0x0c,0xe3};
+const char QB_UUID_SPH_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68d043";
+// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+2,0x1f,0x0c,0xe3};
+const char QB_UUID_ACC_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68e043";
+// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+3,0x1f,0x0c,0xe3};
+const char QB_UUID_GYR_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68f043";
+// {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6+4,0x1f,0x0c,0xe3};
+
+uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 const std::complex<float>i(0, 1);
 
 // TODO manage namespaces better
@@ -356,8 +356,8 @@ class Qbead {
       :
         pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
-  
-  static Qbead *singletoninstance;
+
+  static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
 
   // LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
@@ -375,6 +375,7 @@ class Qbead {
   float T_freeze = 0;
   float T_shaking = 0;
   float shakingCounter = 0;
+  float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble;
   bool frozen = false; // frozen means that there is an animation in progress
   bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
@@ -455,6 +456,7 @@ class Qbead {
   {
     blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+    blecharacc->setValue(zerobuffer20, 3*sizeof(float));
   }
 
   // TODO: Check when the new flex-pcb has arrived
@@ -581,9 +583,22 @@ class Qbead {
     void onWrite(BLECharacteristic *pCharacteristic) {
       Serial.println("[INFO]{BLE} Received a write on the color characteristic");
       uint8_t* pData = pCharacteristic->getData();
-      Qbead::singletoninstance->c_ble =  (pData[2] << 16) | (pData[1] << 8) | pData[0];
-      Serial.print("[DEBUG]{BLE} Received");
-      Serial.println(Qbead::singletoninstance->c_ble, HEX);
+      singletoninstance->c_ble =  (pData[2] << 16) | (pData[1] << 8) | pData[0];
+      Serial.print("[DEBUG]{BLE}Qbead received");
+      Serial.println(singletoninstance->c_ble, HEX);
+    }
+  };
+  
+  class ThetaPhiCharCallbacks: public BLECharacteristicCallbacks {
+    void onWrite(BLECharacteristic *pCharacteristic) {
+      Serial.println("[INFO]{BLE} Received a write on the spherical coordinates characteristic");
+      uint8_t* pData = pCharacteristic->getData();
+      singletoninstance->t_ble = ((uint32_t)pData[0])*180/255;
+      singletoninstance->p_ble = ((uint32_t)pData[1])*360/255;
+      Serial.print("[DEBUG]{BLE} Received t=");
+      Serial.print(singletoninstance->t_ble);
+      Serial.print(" p=");
+      Serial.println(singletoninstance->p_ble);
     }
   };
 
@@ -623,15 +638,25 @@ class Qbead {
     blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
     blecharcol->setCallbacks(new ColorCharCallbacks());
+    blecharcol->setValue(zerobuffer20, 3);
     
     blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
-    
-    
+    blecharsph->setCallbacks(new ThetaPhiCharCallbacks());
+    blecharsph->setValue(zerobuffer20, 2);
+
     blechargyr = bleservice->createCharacteristic(QB_UUID_GYR_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
-
-    bleservice->start();
+    blechargyr->setValue(zerobuffer20, 3*sizeof(float));
+                  
+    startAccelerometer();
+
+    if (bleservice) {
+      Serial.println("starting service");
+      bleservice->start();
+    } else {
+      Serial.println("Service is null!");
+    }
     startBLEadv();
     // Tap detection
     // setupTapInterrupt();
@@ -880,8 +905,14 @@ class Qbead {
 
   void writeToBLE(BLECharacteristic* destination, Vector3d vector) {
     float buffer[] = {(float)vector(0), (float)vector(1), (float)vector(2)};
-    destination->setValue((uint8_t*)buffer, sizeof(buffer));
-    destination->notify();
+    if (destination) 
+    {
+      destination->setValue((uint8_t*)buffer, sizeof(buffer));
+      destination->notify();
+    } else
+    {
+      Serial.println("destination is null");
+    }
   }
 
   Vector3d getVectorFromBuffer(float *buffer) {
@@ -935,8 +966,12 @@ class Qbead {
       Serial.println(gyroVector(2));
     }
     
+    if (blecharacc) {
     writeToBLE(blecharacc, gravityVector);
+    }
+    if (blechargyr) {
     writeToBLE(blechargyr, gyroVector);
+    }
   }
 }; // end class
 

From c0cf0372629b68b55cbaf43723b9291a63babad0 Mon Sep 17 00:00:00 2001
From: Victor <131750770+pitkro@users.noreply.github.com>
Date: Tue, 3 Jun 2025 15:19:57 +0200
Subject: [PATCH 50/64] added descriptors to allow for notifications and
 identification of char

---
 src_ESP32/QbeadESP32.h | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 040d72a..3a7a366 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -9,6 +9,7 @@
 #include <BLEDevice.h>
 #include <BLEUtils.h>
 #include <BLEServer.h>
+#include <BLE2902.h>
 
 using namespace Eigen;
 
@@ -456,6 +457,10 @@ class Qbead {
   {
     blecharacc = bleservice->createCharacteristic(QB_UUID_ACC_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+    BLEDescriptor* pAccDesc = new BLEDescriptor("2901");
+    pAccDesc->setValue("Accelerometer readout Characteristic");
+    blecharacc->addDescriptor(pAccDesc);
+    blecharacc->addDescriptor(new BLE2902());
     blecharacc->setValue(zerobuffer20, 3*sizeof(float));
   }
 
@@ -637,16 +642,26 @@ class Qbead {
     float zerobufferfloat[] = {0.0f, 0.0f, 0.0f};
     blecharcol = bleservice->createCharacteristic(QB_UUID_COL_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    BLEDescriptor* pColDesc = new BLEDescriptor("2901");
+    pColDesc->setValue("Color Characteristic");
+    blecharcol->addDescriptor(pColDesc);
     blecharcol->setCallbacks(new ColorCharCallbacks());
     blecharcol->setValue(zerobuffer20, 3);
     
     blecharsph = bleservice->createCharacteristic(QB_UUID_SPH_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_WRITE);
+    BLEDescriptor* pSphDesc = new BLEDescriptor("2901");
+    pSphDesc->setValue("Theta and Phi Characteristic");
+    blecharsph->addDescriptor(pSphDesc);
     blecharsph->setCallbacks(new ThetaPhiCharCallbacks());
     blecharsph->setValue(zerobuffer20, 2);
 
     blechargyr = bleservice->createCharacteristic(QB_UUID_GYR_CHAR,
                   BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
+    BLEDescriptor* pGyrDesc = new BLEDescriptor("2901");
+    pGyrDesc->setValue("Gyroscope readout Characteristic");
+    blechargyr->addDescriptor(pGyrDesc);
+    blechargyr->addDescriptor(new BLE2902());
     blechargyr->setValue(zerobuffer20, 3*sizeof(float));
                   
     startAccelerometer();

From a986a95755e6e17910bf56f5e76415eee112875a Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 3 Jun 2025 15:52:42 +0200
Subject: [PATCH 51/64] Enable imu

---
 src_ESP32/QbeadESP32.h | 24 +++++++++---------------
 1 file changed, 9 insertions(+), 15 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 1a9cb87..c725042 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -16,7 +16,7 @@ using namespace Eigen;
 // default configs
 #define QB_LEDPIN 9
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
-#define QB_IMU_ADDR 0x6A
+#define QB_IMU_ADDR 0x68
 #define QB_IX 1
 #define QB_IY 0
 #define QB_IZ 2
@@ -353,13 +353,13 @@ class Qbead {
   Qbead(const uint16_t pin00 = QB_LEDPIN,
         const uint16_t pixelconfig = QB_PIXELCONFIG,
         const uint8_t imu_addr = QB_IMU_ADDR)
-      :
+      : imu(LSM6DS3(I2C_MODE, imu_addr)),
         pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
 
   static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
 
-  // LSM6DS3 imu;
+  LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
   BLEServer* bleserver;
@@ -938,18 +938,12 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    // rbuffer[0] = imu.readFloatAccelX();
-    // rbuffer[1] = imu.readFloatAccelY();
-    // rbuffer[2] = imu.readFloatAccelZ();    
-    // rgyrobuffer[0] = imu.readFloatGyroX();
-    // rgyrobuffer[1] = imu.readFloatGyroY();
-    // rgyrobuffer[2] = imu.readFloatGyroZ();
-    rbuffer[0] = 0;
-    rbuffer[1] = 0;
-    rbuffer[2] = 1; // gravity vector
-    rgyrobuffer[0] = 0;
-    rgyrobuffer[1] = 0;
-    rgyrobuffer[2] = 0; // gyro vector
+    rbuffer[0] = imu.readFloatAccelX();
+    rbuffer[1] = imu.readFloatAccelY();
+    rbuffer[2] = imu.readFloatAccelZ();    
+    rgyrobuffer[0] = imu.readFloatGyroX();
+    rgyrobuffer[1] = imu.readFloatGyroY();
+    rgyrobuffer[2] = imu.readFloatGyroZ();
 
     float T_new = micros();
     float delta = T_new - T_imu;

From 464da99b7b604b9ceb66aa41a65db5bd017a34fb Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 3 Jun 2025 15:55:36 +0200
Subject: [PATCH 52/64] Fix merge

---
 src/Qbead.h            | 2 +-
 src_ESP32/QbeadESP32.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/Qbead.h b/src/Qbead.h
index d294037..ded6b2a 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -270,7 +270,7 @@ class QuantumState
 
   void collapse()
   {
-    const float a = (stateCoordinates.z() + 1) / 2; // probability of measuring |0>
+    const float a = (stateCoordinates.v(2) + 1) / 2; // probability of measuring |0>
     if (a < 0.0001) {
       stateCoordinates.set(0, 0, -1);
       return;
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index c725042..fc07d19 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -261,7 +261,7 @@ class QuantumState
 
   void collapse()
   {
-    const float a = (stateCoordinates.z() + 1) / 2; // probability of measuring |0>
+    const float a = (stateCoordinates.v(2) + 1) / 2; // probability of measuring |0>
     if (a < 0.0001) {
       stateCoordinates.set(0, 0, -1);
       return;

From 20396fee1588e8a3e6c74cd4519b97265b05b68f Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 3 Jun 2025 16:01:16 +0200
Subject: [PATCH 53/64] Set led pin

---
 src_ESP32/QbeadESP32.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index fc07d19..d514c6c 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -14,7 +14,7 @@
 using namespace Eigen;
 
 // default configs
-#define QB_LEDPIN 9
+#define QB_LEDPIN 21
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
 #define QB_IMU_ADDR 0x68
 #define QB_IX 1

From 7ad57f2dc74542a3cd81c1a7b20f2899d54d5dcb Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Wed, 4 Jun 2025 15:29:42 +0200
Subject: [PATCH 54/64] Use correct imu driver

---
 examples/Decoherence/Decoherence.ino |  2 +-
 src_ESP32/QbeadESP32.h               | 26 ++++++++++++++++----------
 2 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino
index 61dc9f5..c88c285 100644
--- a/examples/Decoherence/Decoherence.ino
+++ b/examples/Decoherence/Decoherence.ino
@@ -37,7 +37,7 @@ void setup()
 
 void loop()
 {
-    bead.readIMU(false);
+    bead.readIMU(true);
     bead.clear();
     bead.showAxis();
     stateColor = color(255, 255, 255);
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index d514c6c..cac2047 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -3,7 +3,8 @@
 
 #include <Arduino.h>
 #include <Adafruit_NeoPixel.h>
-#include <LSM6DS3.h>
+#include <Wire.h>
+#include <ICM_20948.h>
 #include <math.h>
 #include <ArduinoEigen.h>
 #include <BLEDevice.h>
@@ -29,6 +30,8 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
+ICM_20948_I2C imu;
+
 const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
 // {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
 const char QB_UUID_COL_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68c043";
@@ -353,13 +356,11 @@ class Qbead {
   Qbead(const uint16_t pin00 = QB_LEDPIN,
         const uint16_t pixelconfig = QB_PIXELCONFIG,
         const uint8_t imu_addr = QB_IMU_ADDR)
-      : imu(LSM6DS3(I2C_MODE, imu_addr)),
-        pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
+      : pixels(Adafruit_NeoPixel(QB_PIXEL_COUNT, pin00, pixelconfig))
   {}
 
   static Qbead *singletoninstance; // we need a global singleton static instance because bluefruit callbacks do not support context variables -- thankfully this is fine because there is indeed only one Qbead in existence at any time
 
-  LSM6DS3 imu;
   Adafruit_NeoPixel pixels;
 
   BLEServer* bleserver;
@@ -609,6 +610,8 @@ class Qbead {
   void
   begin()
   {
+    Wire.begin(40, 39);
+    Wire.setClock(100000);  // drop to 100kHz
     singletoninstance = this;
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
@@ -676,6 +679,8 @@ class Qbead {
     // setupTapInterrupt();
     // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
     // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
+    delay(100); // wait for the I2C bus to stabilize
+    imu.begin(Wire, QB_IMU_ADDR);
   }
 
   void startBLEadv(void)
@@ -938,12 +943,13 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    rbuffer[0] = imu.readFloatAccelX();
-    rbuffer[1] = imu.readFloatAccelY();
-    rbuffer[2] = imu.readFloatAccelZ();    
-    rgyrobuffer[0] = imu.readFloatGyroX();
-    rgyrobuffer[1] = imu.readFloatGyroY();
-    rgyrobuffer[2] = imu.readFloatGyroZ();
+    imu.getAGMT();
+    rbuffer[0] = imu.accX();
+    rbuffer[1] = imu.accY();
+    rbuffer[2] = imu.accZ();  
+    rgyrobuffer[0] = imu.gyrX();
+    rgyrobuffer[1] = imu.gyrY();
+    rgyrobuffer[2] = imu.gyrZ();
 
     float T_new = micros();
     float delta = T_new - T_imu;

From 569fea3f030372921fa390fa6b6ed190081a1706 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Thu, 5 Jun 2025 12:14:41 +0200
Subject: [PATCH 55/64] Fix some imu issues

---
 src_ESP32/QbeadESP32.h | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index cac2047..b4ae024 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -17,7 +17,7 @@ using namespace Eigen;
 // default configs
 #define QB_LEDPIN 21
 #define QB_PIXELCONFIG NEO_BRG + NEO_KHZ800
-#define QB_IMU_ADDR 0x68
+#define QB_IMU_ADDR 0x69
 #define QB_IX 1
 #define QB_IY 0
 #define QB_IZ 2
@@ -611,7 +611,7 @@ class Qbead {
   begin()
   {
     Wire.begin(40, 39);
-    Wire.setClock(100000);  // drop to 100kHz
+    Wire.setClock(50000);  // drop to 50kHz
     singletoninstance = this;
     Serial.begin(9600);
     for (int waitCount = 0; waitCount < 50; waitCount++)
@@ -943,10 +943,14 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
+    while (!imu.dataReady()) {
+      delay(20);  // 1-2 ms delay is fine
+    }
+
     imu.getAGMT();
-    rbuffer[0] = imu.accX();
-    rbuffer[1] = imu.accY();
-    rbuffer[2] = imu.accZ();  
+    rbuffer[0] = imu.accX() / 1000.0f; // convert to g
+    rbuffer[1] = imu.accY() / 1000.0f;
+    rbuffer[2] = imu.accZ() / 1000.0f;  
     rgyrobuffer[0] = imu.gyrX();
     rgyrobuffer[1] = imu.gyrY();
     rgyrobuffer[2] = imu.gyrZ();

From 83c0b6d859ba6daa45a63868068e81371ae8496d Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 6 Jun 2025 14:39:06 +0200
Subject: [PATCH 56/64] Add acc

---
 src_ESP32/QbeadESP32.h | 58 ++++++++++++++----------------------------
 1 file changed, 19 insertions(+), 39 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index b4ae024..9809fc8 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -5,6 +5,7 @@
 #include <Adafruit_NeoPixel.h>
 #include <Wire.h>
 #include <ICM_20948.h>
+#include <ICM20948_WE.h>
 #include <math.h>
 #include <ArduinoEigen.h>
 #include <BLEDevice.h>
@@ -30,8 +31,6 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
-ICM_20948_I2C imu;
-
 const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
 // {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
 const char QB_UUID_COL_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68c043";
@@ -46,6 +45,8 @@ const char QB_UUID_GYR_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68f043";
 uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 const std::complex<float>i(0, 1);
 
+ICM20948_WE imu;
+
 // TODO manage namespaces better
 // The setPixelColor switches blue and green
 static uint32_t color(uint8_t r, uint8_t g, uint8_t b) {
@@ -371,7 +372,6 @@ class Qbead {
   BLECharacteristic* blechargyr;
   BLEAdvertising* bleadvertising;
 
-  float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
@@ -625,16 +625,8 @@ class Qbead {
     setBrightness(10);
 
     Serial.println("[INFO] Booting... Qbead on XIAO ESP32 compiled on " __DATE__ " at " __TIME__);
-    // if (!imu.begin()) {
-    //   Serial.println("[DEBUG]{IMU} IMU initialized correctly");
-    // } else {
-    //   Serial.println("[ERROR]{IMU} IMU failed to initialize");
-    // }
 
     BLEDevice::init("qbead | " __DATE__ " " __TIME__);
-    // Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
-    // Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
-    // Bluefruit.Periph.setConnectCallback(connect_callback);
     bleserver = BLEDevice::createServer();
     bleserver->setCallbacks(new MyServerCallbacks());
     bleservice = bleserver->createService(QB_UUID_SERVICE);
@@ -679,8 +671,10 @@ class Qbead {
     // setupTapInterrupt();
     // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
     // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
-    delay(100); // wait for the I2C bus to stabilize
-    imu.begin(Wire, QB_IMU_ADDR);
+    imu = ICM20948_WE(&Wire, QB_IMU_ADDR);
+    imu.setGyrRange(ICM20948_GYRO_RANGE_2000);
+    imu.setAccDLPF(ICM20948_DLPF_6);
+    imu.setAccRange(ICM20948_ACC_RANGE_16G);
   }
 
   void startBLEadv(void)
@@ -693,12 +687,6 @@ class Qbead {
     advertisementData.setName("qbead | " __DATE__ " " __TIME__);
     advertisementData.setFlags(6); // BLE_SIG_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE = 6
 
-    // Bluefruit.Advertising.addTxPower();
-
-    // Secondary Scan Response packet (optional)
-    // Since there is no room for 'Name' in Advertising packet
-    // Bluefruit.ScanResponse.addName();
-
     /* Start Advertising
     * - Enable auto advertising if disconnected
     * - Interval:  fast mode = 20 ms, slow mode = 152.5 ms
@@ -714,9 +702,6 @@ class Qbead {
     bleadvertising->setMaxInterval(244);
   
     BLEDevice::startAdvertising();
-    // Bluefruit.Advertising.setInterval(32, 244);    // in unit of 0.625 ms
-    // Bluefruit.Advertising.setFastTimeout(30);      // number of seconds in fast mode
-    // Bluefruit.Advertising.start(0);                // 0 = Don't stop advertising after n seconds
   }
 
   void clear() {
@@ -934,7 +919,8 @@ class Qbead {
     }
   }
 
-  Vector3d getVectorFromBuffer(float *buffer) {
+  Vector3d getVector(xyzFloat* xyz) {
+    float buffer[3] = {xyz->x, xyz->y, xyz->z};
     // calibration of imu because imu is not aligned with bloch sphere
     float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
     float ry = (1 - 2 * QB_SY) * buffer[QB_IY];
@@ -943,27 +929,21 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    while (!imu.dataReady()) {
-      delay(20);  // 1-2 ms delay is fine
-    }
-
-    imu.getAGMT();
-    rbuffer[0] = imu.accX() / 1000.0f; // convert to g
-    rbuffer[1] = imu.accY() / 1000.0f;
-    rbuffer[2] = imu.accZ() / 1000.0f;  
-    rgyrobuffer[0] = imu.gyrX();
-    rgyrobuffer[1] = imu.gyrY();
-    rgyrobuffer[2] = imu.gyrZ();
+    xyzFloat gyr;
+    xyzFloat acc;
+    imu.readSensor();
+    imu.getGyrValues(&gyr);
+    imu.getAccRawValues(&acc);
 
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
 
-    Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
+    Vector3d newGyro = getVector(&gyr) * PI / 180;
     float d = min(delta / float(T_GYRO), 1.0f);
     gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
 
-    Vector3d newGravity = getVectorFromBuffer(rbuffer);
+    Vector3d newGravity = getVector(&acc) / 1000.0f; // convert to g
     d = min(delta / float(T_ACC), 1.0f);
     gravityVector = d * newGravity + (1 - d) * gravityVector;
 
@@ -985,10 +965,10 @@ class Qbead {
     }
     
     if (blecharacc) {
-    writeToBLE(blecharacc, gravityVector);
+      writeToBLE(blecharacc, gravityVector);
     }
     if (blechargyr) {
-    writeToBLE(blechargyr, gyroVector);
+      writeToBLE(blechargyr, gyroVector);
     }
   }
 }; // end class
@@ -997,4 +977,4 @@ Qbead *Qbead::singletoninstance = nullptr;
 
 } // end namespace
 
-#endif // QBEAD_H 
\ No newline at end of file
+#endif // QBEAD_H
\ No newline at end of file

From 98efca0f5528f497d0cf85ada1f05c471f2a7446 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 6 Jun 2025 14:45:58 +0200
Subject: [PATCH 57/64] Fix shaking

---
 src_ESP32/QbeadESP32.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 9809fc8..11c014c 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -674,7 +674,7 @@ class Qbead {
     imu = ICM20948_WE(&Wire, QB_IMU_ADDR);
     imu.setGyrRange(ICM20948_GYRO_RANGE_2000);
     imu.setAccDLPF(ICM20948_DLPF_6);
-    imu.setAccRange(ICM20948_ACC_RANGE_16G);
+    imu.setAccRange(ICM20948_ACC_RANGE_8G);
   }
 
   void startBLEadv(void)

From ea0ddc872af222a266ca54010cba2e6519de4cda Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Fri, 6 Jun 2025 15:52:14 +0200
Subject: [PATCH 58/64] Fix imu sometimes not working

---
 src_ESP32/QbeadESP32.h | 49 ++++++++++++++++++++++++++----------------
 1 file changed, 31 insertions(+), 18 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 11c014c..3463fc5 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -31,6 +31,8 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 
+ICM_20948_I2C imu;
+
 const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
 // {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
 const char QB_UUID_COL_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68c043";
@@ -44,8 +46,7 @@ const char QB_UUID_GYR_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68f043";
 
 uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 const std::complex<float>i(0, 1);
-
-ICM20948_WE imu;
+ICM20948_WE myImu;
 
 // TODO manage namespaces better
 // The setPixelColor switches blue and green
@@ -372,6 +373,7 @@ class Qbead {
   BLECharacteristic* blechargyr;
   BLEAdvertising* bleadvertising;
 
+  float rbuffer[3], rgyrobuffer[3];
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
@@ -627,6 +629,9 @@ class Qbead {
     Serial.println("[INFO] Booting... Qbead on XIAO ESP32 compiled on " __DATE__ " at " __TIME__);
 
     BLEDevice::init("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.begin(QB_MAX_PRPH_CONNECTION, 0);
+    // Bluefruit.setName("qbead | " __DATE__ " " __TIME__);
+    // Bluefruit.Periph.setConnectCallback(connect_callback);
     bleserver = BLEDevice::createServer();
     bleserver->setCallbacks(new MyServerCallbacks());
     bleservice = bleserver->createService(QB_UUID_SERVICE);
@@ -671,10 +676,13 @@ class Qbead {
     // setupTapInterrupt();
     // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
     // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
-    imu = ICM20948_WE(&Wire, QB_IMU_ADDR);
-    imu.setGyrRange(ICM20948_GYRO_RANGE_2000);
-    imu.setAccDLPF(ICM20948_DLPF_6);
-    imu.setAccRange(ICM20948_ACC_RANGE_8G);
+    delay(100); // wait for the I2C bus to stabilize
+    imu.begin(Wire, QB_IMU_ADDR);
+
+    myImu = ICM20948_WE(&Wire, QB_IMU_ADDR);
+    myImu.setGyrRange(ICM20948_GYRO_RANGE_2000);
+    myImu.setAccDLPF(ICM20948_DLPF_6);
+    myImu.setAccRange(ICM20948_ACC_RANGE_8G);
   }
 
   void startBLEadv(void)
@@ -919,8 +927,7 @@ class Qbead {
     }
   }
 
-  Vector3d getVector(xyzFloat* xyz) {
-    float buffer[3] = {xyz->x, xyz->y, xyz->z};
+  Vector3d getVectorFromBuffer(float *buffer) {
     // calibration of imu because imu is not aligned with bloch sphere
     float rx = (1 - 2 * QB_SX) * buffer[QB_IX];
     float ry = (1 - 2 * QB_SY) * buffer[QB_IY];
@@ -929,21 +936,27 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    xyzFloat gyr;
-    xyzFloat acc;
-    imu.readSensor();
-    imu.getGyrValues(&gyr);
-    imu.getAccRawValues(&acc);
+    while (!imu.dataReady()) {
+      delay(20);  // 1-2 ms delay is fine
+    }
+
+    imu.getAGMT();
+    rbuffer[0] = imu.accX() / 1000.0f; // convert to g
+    rbuffer[1] = imu.accY() / 1000.0f;
+    rbuffer[2] = imu.accZ() / 1000.0f;  
+    rgyrobuffer[0] = imu.gyrX();
+    rgyrobuffer[1] = imu.gyrY();
+    rgyrobuffer[2] = imu.gyrZ();
 
     float T_new = micros();
     float delta = T_new - T_imu;
     T_imu = T_new;
 
-    Vector3d newGyro = getVector(&gyr) * PI / 180;
+    Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
     float d = min(delta / float(T_GYRO), 1.0f);
     gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
 
-    Vector3d newGravity = getVector(&acc) / 1000.0f; // convert to g
+    Vector3d newGravity = getVectorFromBuffer(rbuffer);
     d = min(delta / float(T_ACC), 1.0f);
     gravityVector = d * newGravity + (1 - d) * gravityVector;
 
@@ -965,10 +978,10 @@ class Qbead {
     }
     
     if (blecharacc) {
-      writeToBLE(blecharacc, gravityVector);
+    writeToBLE(blecharacc, gravityVector);
     }
     if (blechargyr) {
-      writeToBLE(blechargyr, gyroVector);
+    writeToBLE(blechargyr, gyroVector);
     }
   }
 }; // end class
@@ -977,4 +990,4 @@ Qbead *Qbead::singletoninstance = nullptr;
 
 } // end namespace
 
-#endif // QBEAD_H
\ No newline at end of file
+#endif // QBEAD_H 
\ No newline at end of file

From 1372cf1c21bf5f0e74b0a1ad4c5cc30411206eab Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 10 Jun 2025 10:17:20 +0200
Subject: [PATCH 59/64] Fix shaking

---
 src_ESP32/QbeadESP32.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 3463fc5..33053b1 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -832,7 +832,7 @@ class Qbead {
       {
         return false;
       }
-      if (totalAcceleration > 11)
+      if (totalAcceleration > 3)
       {
         Serial.println("Randomizing");
         float randomTheta = (random(0, 1000)/1000.0f) * PI;
@@ -850,7 +850,7 @@ class Qbead {
       }
       return false;
     }
-    if (totalAcceleration > 11)
+    if (totalAcceleration > 3)
     {
       Serial.print("Detected shaking turning on shakingState, acc length: ");
       Serial.println(totalAcceleration);
@@ -990,4 +990,4 @@ Qbead *Qbead::singletoninstance = nullptr;
 
 } // end namespace
 
-#endif // QBEAD_H 
\ No newline at end of file
+#endif // QBEAD_H
\ No newline at end of file

From d5a0bb65d81183c7f8104eba5cf92daffaea587c Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 10 Jun 2025 11:59:29 +0200
Subject: [PATCH 60/64] Add double tap support

---
 src_ESP32/QbeadESP32.h | 169 ++++++++++++++++++++++-------------------
 1 file changed, 90 insertions(+), 79 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 33053b1..a1717de 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -25,13 +25,14 @@ using namespace Eigen;
 #define QB_SX 0
 #define QB_SY 0
 #define QB_SZ 1
-#define GYRO_GATE_THRESHOLD 8
+#define GYRO_GATE_THRESHOLD 12
 #define QB_PIXEL_COUNT 62
 #define QB_MAX_PRPH_CONNECTION 2
 #define T_ACC 100000
 #define T_GYRO 10000
-
-ICM_20948_I2C imu;
+#define TAP_THRESHOLD_TIME 400 // Threshold for tap detection in milliseconds
+#define TAP_THRESHOLD 8 // Threshold for tap detection in g/s
+#define DEBOUNCE_TIME 50 // Debounce time in milliseconds
 
 const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
 // {0x45,0x8d,0x08,0xaa,0xd6,0x63,0x44,0x25,0xbe,0x12,0x9c,0x35,0xc6,0x1f,0x0c,0xe3};
@@ -46,7 +47,9 @@ const char QB_UUID_GYR_CHAR[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68f043";
 
 uint8_t zerobuffer20[] = {0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
 const std::complex<float>i(0, 1);
-ICM20948_WE myImu;
+// We need both because ICM_20948_I2C does not support changing the gyro range
+ICM20948_WE imuWE;
+ICM_20948_I2C imuI2C;
 
 // TODO manage namespaces better
 // The setPixelColor switches blue and green
@@ -138,29 +141,6 @@ void sphericalToCartesian(float theta, float phi, float& x, float& y, float& z)
   z = cos(theta);
 }
 
-// Tap detection
-// LSM6DS3 myIMU(I2C_MODE, QB_IMU_ADDR); // Create an instance of the IMU
-uint8_t interruptCount = 0; // Amount of received interrupts
-uint8_t prevInterruptCount = 0; // Interrupt Counter from last loop
-
-void setupTapInterrupt() {
-  uint8_t error = 0;
-  uint8_t dataToWrite = 0;
-
-  // Double Tap Config
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x60);
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_CFG1, 0x8E);// INTERRUPTS_ENABLE, SLOPE_FDS
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_TAP_THS_6D, 0x6C);
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_INT_DUR2, 0x7F);
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_WAKE_UP_THS, 0x80);
-  // myIMU.writeRegister(LSM6DS3_ACC_GYRO_MD1_CFG, 0x08);
-}
-
-void int1ISR()
-{
-  interruptCount++;
-}
-
 namespace Qbead {
 
 class Coordinates
@@ -385,8 +365,12 @@ class Qbead {
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
   Coordinates visualState = Coordinates(-0.866, 0.25, -0.433);
   Vector3d gravityVector = Vector3d(0, 0, 1);
+  Vector3d oldGravityVector = Vector3d(0, 0, 1);
   Vector3d gyroVector = Vector3d(0, 0, 1);
-  float yaw = 0;
+  float lastTapTime = 0;
+  float lastDebounceTime = 0; // last time the tap was debounced
+  bool waitingForSecondTap = false;
+  float dt = 0; // time since the last IMU update
 
   // led map index to Coordinates
   // This map is for the first version of the flex-pcb
@@ -672,17 +656,12 @@ class Qbead {
       Serial.println("Service is null!");
     }
     startBLEadv();
-    // Tap detection
-    // setupTapInterrupt();
-    // pinMode(PIN_LSM6DS3TR_C_INT1, INPUT);
-    // attachInterrupt(digitalPinToInterrupt(PIN_LSM6DS3TR_C_INT1), int1ISR, RISING);
-    delay(100); // wait for the I2C bus to stabilize
-    imu.begin(Wire, QB_IMU_ADDR);
-
-    myImu = ICM20948_WE(&Wire, QB_IMU_ADDR);
-    myImu.setGyrRange(ICM20948_GYRO_RANGE_2000);
-    myImu.setAccDLPF(ICM20948_DLPF_6);
-    myImu.setAccRange(ICM20948_ACC_RANGE_8G);
+    
+    imuI2C.begin(Wire, QB_IMU_ADDR);
+    imuWE = ICM20948_WE(&Wire, QB_IMU_ADDR);
+    imuWE.setGyrRange(ICM20948_GYRO_RANGE_2000);
+    imuWE.setAccDLPF(ICM20948_DLPF_6);
+    imuWE.setAccRange(ICM20948_ACC_RANGE_8G);
   }
 
   void startBLEadv(void)
@@ -705,7 +684,6 @@ class Qbead {
     * https://developer.apple.com/library/content/qa/qa1931/_index.html
     */
     bleadvertising->setAdvertisementData(advertisementData);
-    // Bluefruit.Advertising.restartOnDisconnect(true);
     bleadvertising->setMinInterval(32);
     bleadvertising->setMaxInterval(244);
   
@@ -788,11 +766,7 @@ class Qbead {
 
   void animateTo(uint8_t gate, uint16_t animationLength = 2000)
   {
-    if (frozen)
-    {
-      prevInterruptCount = interruptCount;
-    }
-    else if (gate == 0)
+    if (gate == 0)
     {
       return;
     }
@@ -841,7 +815,6 @@ class Qbead {
         setLed(state.getCoordinates(), color(255, 0, 255));
         show();
         shakingState = false;
-        prevInterruptCount = interruptCount;
         return true;
       }
       if (shakingCounter > 800)
@@ -861,6 +834,48 @@ class Qbead {
     return false;
   }
 
+  bool detectDoubleTap(float acc)
+  {
+    float currentTime = millis();
+
+    // Check for tap condition
+    if (abs(acc) > TAP_THRESHOLD)
+    {
+      // Debounce: ensure enough time since last detected tap
+      if (currentTime - lastDebounceTime > DEBOUNCE_TIME)
+      {
+        lastDebounceTime = currentTime;
+
+        if (waitingForSecondTap)
+        {
+          if (currentTime - lastTapTime <= TAP_THRESHOLD_TIME)
+          {
+            waitingForSecondTap = false;
+            return true; // Second tap detected within threshold time
+          }
+          else
+          {
+            // Too late — treat this as new first tap
+            lastTapTime = currentTime;
+          }
+        }
+        else
+        {
+          // First tap detected
+          lastTapTime = currentTime;
+          waitingForSecondTap = true;
+        }
+      }
+    }
+
+    // If waiting too long for second tap, reset state
+    if (waitingForSecondTap && (currentTime - lastTapTime > TAP_THRESHOLD_TIME))
+    {
+      waitingForSecondTap = false;
+    }
+    return false;
+  }
+
   int checkMotion()
   {
     if (frozen)
@@ -878,25 +893,23 @@ class Qbead {
       frozen = false;
       return 0;
     }
-    // Handle tap interrupt
-    if (interruptCount > prevInterruptCount)
+    // Handle double tap
+    float acc = (gravityVector(2) - oldGravityVector(2)) * 1000000 / dt;
+    Serial.print("acc: ");
+    Serial.println(acc);
+    if (detectDoubleTap(acc))
     {
-      uint8_t tapStatus = 0;
-      // myIMU.readRegister(&tapStatus, LSM6DS3_ACC_GYRO_TAP_SRC);
-      prevInterruptCount = interruptCount;
-
-      if (tapStatus & 0x01)
-      {
-        Serial.println("Collapsing");
-        return 8;
-      }
-      else
-      {
-        Serial.println("Executing H gate");
-        return 7;
-      }
+      Serial.println("Collapse detected");
+      return 8; // collapse
+    }
+    float accX = (gravityVector(0) - oldGravityVector(0)) * 1000000 / dt;
+    float accY = (gravityVector(1) - oldGravityVector(1)) * 1000000 / dt;
+    if (detectDoubleTap(accX) || detectDoubleTap(accY))
+    {
+      Serial.println("Hadamard detected");
+      return 7; // Hadamard
     }
-    // Handle shaking
+    // Handle rotating
     for (int i = 0; i < 3; i++)
     {
       if (gyroVector[i] > GYRO_GATE_THRESHOLD)
@@ -936,33 +949,31 @@ class Qbead {
   }
 
   void readIMU(bool print=true) {
-    while (!imu.dataReady()) {
+    while (!imuI2C.dataReady()) {
       delay(20);  // 1-2 ms delay is fine
     }
 
-    imu.getAGMT();
-    rbuffer[0] = imu.accX() / 1000.0f; // convert to g
-    rbuffer[1] = imu.accY() / 1000.0f;
-    rbuffer[2] = imu.accZ() / 1000.0f;  
-    rgyrobuffer[0] = imu.gyrX();
-    rgyrobuffer[1] = imu.gyrY();
-    rgyrobuffer[2] = imu.gyrZ();
+    imuI2C.getAGMT();
+    rbuffer[0] = imuI2C.accX() / 1000.0f; // convert to g
+    rbuffer[1] = imuI2C.accY() / 1000.0f;
+    rbuffer[2] = imuI2C.accZ() / 1000.0f;  
+    rgyrobuffer[0] = imuI2C.gyrX();
+    rgyrobuffer[1] = imuI2C.gyrY();
+    rgyrobuffer[2] = imuI2C.gyrZ();
 
     float T_new = micros();
-    float delta = T_new - T_imu;
+    dt = T_new - T_imu;
     T_imu = T_new;
 
     Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
-    float d = min(delta / float(T_GYRO), 1.0f);
+    float d = min(dt / float(T_GYRO), 1.0f);
     gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
 
     Vector3d newGravity = getVectorFromBuffer(rbuffer);
-    d = min(delta / float(T_ACC), 1.0f);
+    d = min(dt / float(T_ACC), 1.0f);
+    oldGravityVector = gravityVector;
     gravityVector = d * newGravity + (1 - d) * gravityVector;
 
-    yaw += gravityVector.dot(gyroVector);
-    yaw = fmod(yaw, 2 * PI);
-
     if (print) {
       Serial.print(gravityVector(0));
       Serial.print("\t");
@@ -978,10 +989,10 @@ class Qbead {
     }
     
     if (blecharacc) {
-    writeToBLE(blecharacc, gravityVector);
+      writeToBLE(blecharacc, gravityVector);
     }
     if (blechargyr) {
-    writeToBLE(blechargyr, gyroVector);
+      writeToBLE(blechargyr, gyroVector);
     }
   }
 }; // end class

From ac4fe6ea28f4ab1f8decf0329c5a385701b128ff Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 10 Jun 2025 14:47:41 +0200
Subject: [PATCH 61/64] Simplify tap detection

---
 src_ESP32/QbeadESP32.h | 24 ++++++++----------------
 1 file changed, 8 insertions(+), 16 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index a1717de..4f95585 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -838,6 +838,12 @@ class Qbead {
   {
     float currentTime = millis();
 
+    // If waiting too long for second tap, reset state
+    if (waitingForSecondTap && (currentTime - lastTapTime > TAP_THRESHOLD_TIME))
+    {
+      waitingForSecondTap = false;
+    }
+
     // Check for tap condition
     if (abs(acc) > TAP_THRESHOLD)
     {
@@ -848,16 +854,8 @@ class Qbead {
 
         if (waitingForSecondTap)
         {
-          if (currentTime - lastTapTime <= TAP_THRESHOLD_TIME)
-          {
-            waitingForSecondTap = false;
-            return true; // Second tap detected within threshold time
-          }
-          else
-          {
-            // Too late — treat this as new first tap
-            lastTapTime = currentTime;
-          }
+          waitingForSecondTap = false;
+          return true; // Second tap detected within threshold time
         }
         else
         {
@@ -867,12 +865,6 @@ class Qbead {
         }
       }
     }
-
-    // If waiting too long for second tap, reset state
-    if (waitingForSecondTap && (currentTime - lastTapTime > TAP_THRESHOLD_TIME))
-    {
-      waitingForSecondTap = false;
-    }
     return false;
   }
 

From a6c78ba06c6b6cd1fc013bb0818054ff5c3adac3 Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Tue, 10 Jun 2025 15:55:47 +0200
Subject: [PATCH 62/64] Improve shaking

---
 examples/PauliGate_detection/PauliGate_detection.ino | 2 +-
 src/Qbead.h                                          | 7 +------
 src_ESP32/QbeadESP32.h                               | 6 +-----
 3 files changed, 3 insertions(+), 12 deletions(-)

diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index fa6ccf5..c8dc102 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -1,4 +1,4 @@
-#include <QbeadESP32.h>
+#include <Qbead.h>
 
 Qbead::Qbead bead;
 int rotationState = 0;
diff --git a/src/Qbead.h b/src/Qbead.h
index ded6b2a..a59e83b 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -386,7 +386,6 @@ class Qbead {
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
-  float shakingCounter = 0;
   bool frozen = false; // frozen means that there is an animation in progress
   bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
@@ -662,8 +661,7 @@ class Qbead {
     if (shakingState)
     {
       float newTime = millis();
-      shakingCounter += newTime - T_shaking;
-      T_shaking = newTime;
+      float shakingCounter = newTime - T_shaking;
       if (shakingCounter < 300)
       {
         return false;
@@ -675,9 +673,7 @@ class Qbead {
         float randomPhi = (random(0, 1000)/500.0f) * PI;
         state.setCoordinates(Coordinates(randomTheta, randomPhi));
         setLed(state.getCoordinates(), color(255, 0, 255));
-        show();
         shakingState = false;
-        prevInterruptCount = interruptCount;
         return true;
       }
       if (shakingCounter > 800)
@@ -692,7 +688,6 @@ class Qbead {
       Serial.println(totalAcceleration);
       shakingState = true;
       T_shaking = millis();
-      shakingCounter = 0;
     } 
     return false;
   }
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 4f95585..2c343a2 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -357,7 +357,6 @@ class Qbead {
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
-  float shakingCounter = 0;
   float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble;
   bool frozen = false; // frozen means that there is an animation in progress
@@ -800,8 +799,7 @@ class Qbead {
     if (shakingState)
     {
       float newTime = millis();
-      shakingCounter += newTime - T_shaking;
-      T_shaking = newTime;
+      float shakingCounter = newTime - T_shaking;
       if (shakingCounter < 300)
       {
         return false;
@@ -813,7 +811,6 @@ class Qbead {
         float randomPhi = (random(0, 1000)/500.0f) * PI;
         state.setCoordinates(Coordinates(randomTheta, randomPhi));
         setLed(state.getCoordinates(), color(255, 0, 255));
-        show();
         shakingState = false;
         return true;
       }
@@ -829,7 +826,6 @@ class Qbead {
       Serial.println(totalAcceleration);
       shakingState = true;
       T_shaking = millis();
-      shakingCounter = 0;
     } 
     return false;
   }

From 53d7a3328ed0006b2d53ec787cf10a202b0c37dc Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 16 Jun 2025 15:06:21 +0200
Subject: [PATCH 63/64] Improve shake detection

---
 src_ESP32/QbeadESP32.h | 10 +++++++---
 1 file changed, 7 insertions(+), 3 deletions(-)

diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 2c343a2..5fa3353 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -31,7 +31,7 @@ using namespace Eigen;
 #define T_ACC 100000
 #define T_GYRO 10000
 #define TAP_THRESHOLD_TIME 400 // Threshold for tap detection in milliseconds
-#define TAP_THRESHOLD 8 // Threshold for tap detection in g/s
+#define TAP_THRESHOLD 5 // Threshold for tap detection in g/s
 #define DEBOUNCE_TIME 50 // Debounce time in milliseconds
 
 const char QB_UUID_SERVICE[] = "e5eaa0bd-babb-4e8c-a0f8-054ade68b043";
@@ -598,7 +598,7 @@ class Qbead {
     Wire.begin(40, 39);
     Wire.setClock(50000);  // drop to 50kHz
     singletoninstance = this;
-    Serial.begin(9600);
+    Serial.begin(115200);
     for (int waitCount = 0; waitCount < 50; waitCount++)
     {
       if (Serial) {break;}
@@ -826,6 +826,7 @@ class Qbead {
       Serial.println(totalAcceleration);
       shakingState = true;
       T_shaking = millis();
+      waitingForSecondTap = false; // reset double tap detection
     } 
     return false;
   }
@@ -859,6 +860,9 @@ class Qbead {
           lastTapTime = currentTime;
           waitingForSecondTap = true;
         }
+      } else if (currentTime - lastDebounceTime > 30) {
+        // reset because it was a shake, not a tap
+        waitingForSecondTap = false;
       }
     }
     return false;
@@ -938,7 +942,7 @@ class Qbead {
 
   void readIMU(bool print=true) {
     while (!imuI2C.dataReady()) {
-      delay(20);  // 1-2 ms delay is fine
+      delay(1);  // 1-2 ms delay is fine
     }
 
     imuI2C.getAGMT();

From f1cc8a6fc4aa073f895f83f729fc05cba1a1dfbb Mon Sep 17 00:00:00 2001
From: Ard Geuze <geuzeard@gmail.com>
Date: Mon, 23 Jun 2025 12:42:30 +0200
Subject: [PATCH 64/64] Make code faster

---
 examples/Decoherence/Decoherence.ino              | 15 ++++++++++-----
 .../PauliGate_detection/PauliGate_detection.ino   | 13 ++++++++-----
 src/Qbead.h                                       |  9 ++++++---
 src_ESP32/QbeadESP32.h                            |  2 ++
 4 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/examples/Decoherence/Decoherence.ino b/examples/Decoherence/Decoherence.ino
index c88c285..8a034d0 100644
--- a/examples/Decoherence/Decoherence.ino
+++ b/examples/Decoherence/Decoherence.ino
@@ -38,8 +38,6 @@ void setup()
 void loop()
 {
     bead.readIMU(true);
-    bead.clear();
-    bead.showAxis();
     stateColor = color(255, 255, 255);
     Serial.print("rotationState: ");
     Serial.println(rotationState);
@@ -72,10 +70,17 @@ void loop()
         Qbead::Coordinates newCoordinates(bead.state.getCoordinates().theta(), phi);
         bead.state.setCoordinates(newCoordinates);
         bead.visualState = bead.state.getCoordinates();
-        bead.setLed(oldCoordinates, decoherenceColor);
     }
     t = millis();
     bead.animateTo(rotationState, 2000);
-    bead.setLed(bead.visualState, stateColor, 1);
-    bead.show();
+    if (bead.T_led + 20 < millis()) {
+        bead.T_led = millis();
+        bead.clear();
+        if (!bead.frozen) {
+            bead.setLed(oldCoordinates, decoherenceColor);
+        }
+        bead.showAxis();
+        bead.setLed(bead.visualState, stateColor, 1);
+        bead.show();
+    }
 }
\ No newline at end of file
diff --git a/examples/PauliGate_detection/PauliGate_detection.ino b/examples/PauliGate_detection/PauliGate_detection.ino
index c8dc102..08d97fb 100644
--- a/examples/PauliGate_detection/PauliGate_detection.ino
+++ b/examples/PauliGate_detection/PauliGate_detection.ino
@@ -17,7 +17,6 @@ void setup() {
   Serial.println("testing smooth transition between pixels");
   for (int phi = 0; phi < 360; phi += 30) {
     for (int theta = 0; theta < 180; theta += 3) {
-      bead.clear();
       bead.setBloch_deg(theta, phi, colorWheel_deg(phi));
       bead.show();
     }
@@ -27,8 +26,6 @@ void setup() {
 
 void loop() {
   bead.readIMU(false);
-  bead.clear();
-  bead.showAxis();
   stateColor = color(255, 255, 255);
   Serial.print("rotationState: ");
   Serial.println(rotationState);
@@ -46,6 +43,12 @@ void loop() {
     }
   }
   bead.animateTo(rotationState, 2000);
-  bead.setLed(bead.visualState, stateColor);
-  bead.show();
+  // The leds can only be updated every 20ms
+  if (bead.T_led + 20 < millis()) {
+    bead.T_led = millis();
+    bead.clear();
+    bead.showAxis();
+    bead.setLed(bead.visualState, stateColor);
+    bead.show();
+  }
 }
\ No newline at end of file
diff --git a/src/Qbead.h b/src/Qbead.h
index a59e83b..a01179f 100644
--- a/src/Qbead.h
+++ b/src/Qbead.h
@@ -386,6 +386,7 @@ class Qbead {
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
+  float T_led = 0; // last update to the LEDs
   bool frozen = false; // frozen means that there is an animation in progress
   bool shakingState = false; // if ShakingState is 1 detected shaking and if shaking keeps happening randomising state
   QuantumState state = QuantumState(Coordinates(-0.866, 0.25, -0.433));
@@ -393,6 +394,7 @@ class Qbead {
   Vector3d gravityVector = Vector3d(0, 0, 1);
   Vector3d gyroVector = Vector3d(0, 0, 1);
   float yaw = 0;
+  float dt  = 0; // time difference between the last two IMU updates
 
   float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble = 0xffffff; // color as sent over BLE connection
@@ -501,6 +503,7 @@ class Qbead {
     }
 
     pixels.begin();
+    T_led = millis();
     clear();
     setBrightness(10);
 
@@ -775,15 +778,15 @@ class Qbead {
     rgyrobuffer[2] = imu.readFloatGyroZ();
 
     float T_new = micros();
-    float delta = T_new - T_imu;
+    dt = T_new - T_imu;
     T_imu = T_new;
 
     Vector3d newGyro = getVectorFromBuffer(rgyrobuffer) * PI / 180;
-    float d = min(delta / float(T_GYRO), 1.0f);
+    float d = min(dt / float(T_GYRO), 1.0f);
     gyroVector = d * newGyro + (1 - d) * gyroVector; // low pass filter
 
     Vector3d newGravity = getVectorFromBuffer(rbuffer);
-    d = min(delta / float(T_ACC), 1.0f);
+    d = min(dt / float(T_ACC), 1.0f);
     gravityVector = d * newGravity + (1 - d) * gravityVector;
 
     yaw += gravityVector.dot(gyroVector);
diff --git a/src_ESP32/QbeadESP32.h b/src_ESP32/QbeadESP32.h
index 5fa3353..3ddcf59 100644
--- a/src_ESP32/QbeadESP32.h
+++ b/src_ESP32/QbeadESP32.h
@@ -357,6 +357,7 @@ class Qbead {
   float T_imu;             // last update from the IMU
   float T_freeze = 0;
   float T_shaking = 0;
+  float T_led = 0; // last update to the LEDs
   float t_ble, p_ble; // theta and phi as sent over BLE connection
   uint32_t c_ble;
   bool frozen = false; // frozen means that there is an animation in progress
@@ -606,6 +607,7 @@ class Qbead {
     }
 
     pixels.begin();
+    T_led = millis();
     clear();
     setBrightness(10);