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

// vytvoříme objekt s názvem
MPU6050 akcelerometr;

// zadáme konstantu Ludofova čísla
// pro pozdější výpočty
const float pi = 3.141592;
//zde zadejte z kolika vzorku bude
// měření probíhat
const int pocet_vzorku = 100;
// pojmenovýní proměných
int16_t ax, ay, az;
float x, y, z;
int  pocet;
float _angle_x, angle_x, _angle_y, angle_y;
long ax_p, ay_p, az_p;

void setup() {
    // inicializace I2C
    Wire.begin();
    // zvolíme vyšší rychlost pro seriový port
    Serial.begin(38400);
    // inicializace akcelerometru
    akcelerometr.initialize();
    // Když to hodí chybu vypíšeme hlášku
    if (akcelerometr.testConnection());
    Serial.println("Spojeni OK...");
}

void loop() {
    // zjistí všechny hodnoty z akcelerometru
    akcelerometr.getAcceleration(&ax, &ay, &az);
    // sčítáme potřebný počet hodnot
    ax_p = ax_p + ax;
    ay_p = ay_p + ay;
    az_p = az_p + az;
    // pomocné určení počru vzorků
    pocet++;
    // když se dosáhne určeného počtu vzorků
    if (pocet == pocet_vzorku)
    {
     //zjistíme průmerné hodnoty
     x = ax_p/pocet_vzorku;
     y = ay_p/pocet_vzorku;
     z = az_p/pocet_vzorku;
     // vypočteme sklon a náklon
     angle_x = atan2(x, sqrt(square(y) + square(z))      )/(pi/180);
     angle_y = atan2(y, sqrt(square(x) + square(z))      )/(pi/180);
     // vynulujeme hodnoty pro další použití
     pocet = 0;
     ax_p = 0;
     ay_p = 0;
     az_p = 0;
    // vypíšeme výsledky na seriový port
    Serial.print(angle_x);
    Serial.print("\t"); // \t = tabulátor
    Serial.println(angle_y);
    }


}