#include <SPI.h>
#include <SD.h>

const int chipSelect = 4;

#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"





#define LED_R 2
#define LED_G 5
#define LED_B 6

const int MPU=0x68;

float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
///////////////////////////////////   CONFIGURATION   /////////////////////////////

int buffersize=1000;
int acel_deadzone=8;
int giro_deadzone=1;


MPU6050 accelgyro(0x68);

int16_t ax, ay, az,gx, gy, gz,x,y,z;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;
const long interval = 1000;
///////////////////////////////////   SETUP   ////////////////////////////////////

int ledState = LOW;
unsigned long previousMillis = 0;
int pushButton = 8;






void setup() {




 pinMode(LED_R, OUTPUT);
 pinMode(LED_G, OUTPUT);
 pinMode(LED_B, OUTPUT);
 pinMode(pushButton, INPUT);


  Wire.begin();
  Wire.beginTransmission(MPU);
  Wire.write(0x69);
  Wire.write(0);
  Wire.endTransmission(true);



TWBR = 24;



Serial.begin(9600);

while (!Serial) {
    ;
  }
     Serial.print(F("Initializing SD card..."));


  if (!SD.begin(chipSelect)) {
    Serial.println(F("Card failed, or not present"));

    return;
  }
  Serial.println(F("card initialized."));


   accelgyro.initialize();

  Serial.println(F("\nMPU6050 Calibration Sketch"));
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);

  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed" );
  delay(1000);
  if (!accelgyro.testConnection())
      digitalWrite(LED_R,HIGH);
if (accelgyro.testConnection())
      digitalWrite(LED_G,HIGH);


  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);

  if (state==0){
    Serial.println(F("\nReading sensors for first time..."));
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println(F("\nCalculating offsets..."));
    calibration();
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.println(F("\nFINISHED!"));

  }



}




///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {



digitalWrite(LED_B, HIGH);
int buttonState = digitalRead(pushButton);

if( buttonState == HIGH){
 digitalWrite(LED_B,LOW);

 unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {

    previousMillis = currentMillis;


    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }


    digitalWrite(LED_G, ledState);
  }

Wire.beginTransmission(MPU);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU,14,true);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  Tmp=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();

  Serial.print(F("sta savlando"));
   File dataFile = SD.open("datalog.txt", FILE_WRITE);

  if (dataFile) {
    dataFile.print(AcX/mean_az);
    dataFile.print(F("  "));
    dataFile.print(AcY/mean_az);
    dataFile.print(F("  "));
    dataFile.print(AcZ/mean_az);
   dataFile.print(F("  "));
    dataFile.println(Tmp/340.00+36.53);
    dataFile.close();



  }

  else {
    Serial.println(F("error opening datalog.txt"));
  }







digitalWrite(LED_G,LOW);



}
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){

    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2);
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println(F("..."));
 digitalWrite(LED_G,LOW);
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {

    previousMillis = currentMillis;


    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }


    digitalWrite(LED_B, ledState);
  }
    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6)  break;
  }

}