/*
@Author: TONYLABS
@Date: 2012/02/02
*/

#include <Esplora.h>

const uint16_t SAMPLES=64;

int calx=0, caly=0, calz=0;
long sums[3] = {0};
int readings[SAMPLES][3] = {0};
uint8_t index = 0;

void setup() {
  Serial.begin(115200);
}

void loop() {
  int rx, ry, rz;
  rx=Esplora.readAccelerometer(X_AXIS);
  ry=Esplora.readAccelerometer(Y_AXIS);
  rz=Esplora.readAccelerometer(Z_AXIS);

  if (Esplora.readButton(SWITCH_DOWN) == LOW) {
    calx=rx, caly=ry, calz=rz;
  } else {
    rx -= calx, ry -= caly, rz -= calz;
    for (uint8_t i=0; i < 3; ++i) {
      sums[i] -= readings[index][i];
    }
    sums[0] += rx, sums[1] += ry, sums[2] += rz;
    readings[index][0] = rx;
    readings[index][1] = ry;
    readings[index][2] = rz;
    index = (index + 1) % SAMPLES;
    rx = sums[0]/SAMPLES, ry = sums[1]/SAMPLES, rz = sums[2]/SAMPLES;

    Esplora.writeRGB(abs(rx), abs(ry), abs(rz));
  }
}
/*
@end
*/