#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

unsigned long time;
const int chipSelect = 10;

#define LED_PIN 13
bool blinkState = false;
 
void setup()
{    
  //Pripojeni I2C sbernice
  Wire.begin();
  //Nastaví seriovou komunakaci na 9600
  Serial.begin(38400);
 //Inicializace SD karty
  Serial.print("Inicializace SD karty...");
  pinMode(chipSelect, OUTPUT);
 
  if (!SD.begin(chipSelect)) {
    Serial.println("/nKarta nenalezena...");
    return;
  }
  Serial.println("/n Karta inicializovana...");

  //Test MPU6050
  Serial.println("Inicializace I2C zarizeni...");
  accelgyro.initialize();
  //Uprava mericiho rozsahu u akcelerometru
  accelgyro.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);
  accelgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_500);
  
  Serial.println("Testovani pripojeni zarizeni...");
  Serial.println(accelgyro.testConnection() ? "MPU6050 uspesne pripojeno" : "MPU6050 pripojeni neuspesne");

  accelgyro.setXAccelOffset(-3174); //3150
  accelgyro.setYAccelOffset(-5681); //5683
  accelgyro.setZAccelOffset(856); //884
  accelgyro.setXGyroOffset(275); //279
  accelgyro.setYGyroOffset(-152); //151
  accelgyro.setZGyroOffset(71); //64

  pinMode(LED_PIN, OUTPUT);

}


void loop()
{
  //float konstanta_a = 1.02;
  //float konstanta_b = 0.01;
  //Definice hodnot senzitivity pro akcelerometr a gyroskop podle tabulky z datasheetu
  //Akcelerometr:
  //pro rozsah +/-2g je odpovídající hodnota s_a 16384
  //pro rozsah +/-4g je odpovídající hodnota s_a 8192
  //pro rozsah +/-8g je odpovídající hodnota s_a 4096
  //pro rozsah +/-16g je odpovídající hodnota s_a 2048
  //Gyroskop::
  //pro rozsah +/-250st/s je odpovídající hodnota s_g 131
  //pro rozsah +/-500st/s je odpovídající hodnota s_g 65.5
  //pro rozsah +/-1000st/s je odpovídající hodnota s_g 32.8
  //pro rozsah +/-2000st/s je odpovídající hodnota s_g 16.4
  float s_a = 4096;
  float s_g = 65.5;

  time = millis();
 
  //String data = ""; //inicializase promenne data
  //String data = String(ax/s_a) + ("\t") + String(ay/s_a) + ("\t") + String(az/s_a) + ("\t") + String(gx/s_g) + ("\t") + String(gy/s_g) + ("\t") + String(gz/s_g) + ("\t");  
  String data = String(ax) + ("\t") + String(ay) + ("\t") + String(az) + ("\t") + String(gx) + ("\t") + String(gy) + ("\t") + String(gz) + ("\t");
  data = data + ("\t");
  data = data + (time);
  
  //Cteni namerenych dat ze senzoru
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
   
   //Otevreni souboru data.txt
   File dataFile = SD.open("data.txt", FILE_WRITE);
 
  //Pokud soubor uz existuje, zapisujeme data do nej 
  if (dataFile) {
    //Samotny zapis - zapise data a skoci na novy radek
    dataFile.println(data);
    dataFile.close();
   
    Serial.print("Zapisuji do souboru data - ");
    Serial.println(data);
  }  
  //Pri chybe vypise error
  else {
    Serial.println("Chyba - nemohu otevřít soubor !!!");
  }
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);
  
  //Cekani do noveho mereni (1s = 1000)
  delay(0); //20 pro fvz 50Hz
 
}
