#include <SparkFun_ADXL345.h>         // SparkFun ADXL345 Library


#define INTEGRATION_TIME 3000 //3sec
#define ALPHA 0.5f
#define FILTER(In,Out) Out=ALPHA*In+(1-ALPHA)*Out  //lowpass filter



ADXL345 adxl = ADXL345();             // USE FOR I2C COMMUNICATION




  float X=0;
  float Y=0;
  float Z=0;


float OFFSET_X=0;
float OFFSET_Y=0;
float OFFSET_Z=0;
float NSAMPLE=0;





void setup(){
  
  Serial.begin(9600);                 // Start the serial terminal
 
  
  adxl.powerOn();                     // Power on the ADXL345

  adxl.setRangeSetting(2);           // Give the range settings
                                      // Accepted values are 2g, 4g, 8g or 16g
                                      // Higher Values = Wider Measurement Range
                                      // Lower Values = Greater Sensitivity


delay(5000);
 //OFFSET ESTIMATION

 unsigned long t0=millis();


while (millis()-t0<=INTEGRATION_TIME)

{
 int x, y, z;
 adxl.readAccel(&x, &y, &z);

 X=X+float(x);
 Y=Y+float(y);
 Z=Z+float(z);
 NSAMPLE=NSAMPLE+1;
}

OFFSET_X=(X)/(NSAMPLE);
OFFSET_Y=(Y)/(NSAMPLE);
OFFSET_Z=(255.0-(Z)/(NSAMPLE));

   Serial.print(OFFSET_X);
  Serial.print(", ");
  Serial.print(OFFSET_Y);
  Serial.print(", ");
  Serial.print(OFFSET_Z); 

    Serial.print(", ");
  Serial.println(NSAMPLE); 
delay(5000);
}


void loop(){
  
    float Xc, Yc, Zc;
    float RollF=0;
    float PitchF=0;
     int x, y, z;
  adxl.readAccel(&x, &y, &z);         // Read the accelerometer values and store them in variables declared above x,y,z

  Xc=float(x)-OFFSET_X;
  Yc=float(y)-OFFSET_Y;
  Zc=float(z)+OFFSET_Z;


 
 
 float roll = 180*atan(Yc/sqrt(Xc*Xc+Zc*Zc))/PI;  
 float  pitch = 180*atan(Xc/sqrt(Yc*Yc+Zc*Zc))/PI; 

RollF=FILTER(roll,RollF);
PitchF=FILTER(pitch,PitchF);
 
  
  // Output Results to Serial
    
  Serial.print(RollF);
  Serial.print(", ");
  Serial.print(PitchF);
  Serial.print(", ");
  Serial.println(Zc-Zc); 
}