     // Connect i2c accelerometer
     setupAccelerometer();
 
+    // get initial values
+    lastAccTime = micros();
+    float x, y, z;
+    mma.getAcceleration(&x, &y, &z);
+    lastAccValue = x;
+
     digitalWrite(StatusLed, LOW);
 }
 
 // MAINLOOP
 void loop() {
     float x, y, z;
-    while (!accDataReady) {};
-    accDataReady = false;
+    int newAccTime;
+    
+    //if (accDataReady) Bluetooth.println("DToo fast accelerometer");
+    //while (!accDataReady) {};
+    //accDataReady = false;
+    newAccTime = micros();
+
+    // micros overflow or not
+    int dt = (newAccTime < lastAccTime) ?
+        lastAccTime - newAccTime :
+        newAccTime  - lastAccTime;
 
     mma.getAcceleration(&x, &y, &z);
-    if (abs(x) > 0.05) {
-        Serial.println(x);
+
+    xVelocity += trapezoidalRule(lastAccValue, x, dt);
+
+    // Update globals
+    lastAccTime = newAccTime;
+    lastAccValue = x;
+
+    Bluetooth.print(xVelocity);
+    Bluetooth.println(x);
+    
+    //if (abs(x) > 0.05) {
+    //    Serial.println(x);
         //Serial.print(",");
         //Serial.print(y);
         //Serial.print(",");
         //Serial.println(z);
-    }
+    //}
     //Serial.print("A");
     //Serial.println((short)(x * 1000));
 }

    // data ready interrupt
    mma.setInterruptsEnabled(MMA_DATA_READY);
    // all defaults to INT2
    attachInterrupt(digitalPinToInterrupt(DataReadyIntPin), setDataReady, FALLING);

    mma.setActive(true); // settings done
    Serial.println("DAccelerometer active.");

    Bluetooth.println("DBluetooth module active.");
}

float trapezoidalRule(float fa, float fb, int dt) {
    return (float) dt * fa * fb * 0.5;
}

//float rk4(float(*f)(float, float), float dt, float t, float y)
//{
//    float  k1 = dt * f(t, y),
//            k2 = dt * f(t + dt / 2, y + k1 / 2),
//            k3 = dt * f(t + dt / 2, y + k2 / 2),
//            k4 = dt * f(t + dt, y + k3);
//    return y + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
//
//}

