#include <Nokia5110.h>
#include "Wire.h"
#include "Math.h"
#include "I2Cdev.h"
#include "MPU6050.h"

// LCDnokia5110::LCDnokia5110(int rst, int ce, int dc, int in, int clk)
// vytvoří objekt LCDnokia5110 s názvem lcd na pinech 7,6,5,4,3
LCDnokia5110 lcd(7,6,5,4,3);

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

// pomovná proměná pro zobrazení na displej
char data[5] = "    ";

// zde je uložen obrázek pozadí pro diplej (šipky)
const unsigned char background [] = {
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x80, 0x80, 0xC0, 0xC0, 0xE0, 0xE0, 0xF0, 0xF0, 0xF8, 0xF8, 0xFC, 0xFC, 0xF8,
0xF0, 0xF0, 0xE0, 0xE0, 0xC0, 0xC0, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x04, 0x04, 0x06, 0x07, 0x07, 0x07, 0x07, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x07, 0x07, 0x07, 0x07, 0x06, 0x04,
0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x3F, 0x3F,
0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x3F, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80,
0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0xFC, 0xF8, 0xF8, 0xF0, 0xF0, 0xE0,
0xC0, 0xC0, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x7F, 0x3E, 0x3C, 0x1C, 0x18, 0x08, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x1F, 0x1F, 0x0F, 0x07, 0x07, 0x03, 0x03, 0x01, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
};


// 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() {
    // viz lekce 7
    lcd.LcdInitialise();
    lcd.LcdClear();
    lcd.GotoXY(13,1);
    lcd.LcdString("Lesson 26");
    lcd.GotoXY(16,3);
    lcd.LcdString("MPU-6050");
    delay(3000);
    lcd.LcdClear();
    lcd.GotoXY(8,1);
    lcd.LcdString("Searching");
    lcd.GotoXY(13,2);
    lcd.LcdString("MPU6050");
    // inicializace I2C
    Wire.begin();
    // inicializace akcelerometru
    akcelerometr.initialize();
    // Když je spojení OK
    if (akcelerometr.testConnection())
    {
    lcd.GotoXY(13,3);
    lcd.LcdString("...OK");
    }
    //jinak
    else
    {
    lcd.GotoXY(6,3);
    lcd.LcdString("NOT FOUND!");
    }

    delay(500);
    // smaž displej
    lcd.LcdClear();
    // vykresli šipky (pozadí)
    lcd.ShowImage(background,0);
}

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;

    // lcd dipleje předáme číslo již jako string
    sprintf(data, "%d" , (int)(angle_x));
    // a zobrazíme kde chceme
    lcd.GotoXY(55,1);
    lcd.LcdString(data);
    // zobrazíme stupně za číslem
    // a dvě mezery pro případ, aby jsme smazali
    // zbytek po dvouciferném čísle na lcd
    lcd.LcdString("'  ");

    // to samé z druhou hodnotou
    sprintf(data, "%d" , (int)(angle_y));
    lcd.GotoXY(55,4);
    lcd.LcdString(data);
    lcd.LcdString("'  ");

    }


}