#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

Adafruit_MPU6050 mpu;

// LED piny
const int ledPins[8] = {2,3,4,5,6,7,8,9};

// Tlačítko nové nuly
const int buttonPin = 10;

// Buzzer
const int buzzerPin = 11;

// Uložená rovina
float zeroAngle = 0;

// Stav tlačítka
bool lastButtonState = HIGH;

// Čas pro přerušované pípání
unsigned long lastBeepTime = 0;

void beep(int duration) {
  tone(buzzerPin, 1000);
  delay(duration);
  noTone(buzzerPin);
}

void setup() {

  Serial.begin(115200);

  // LED
  for(int i = 0; i < 8; i++) {
    pinMode(ledPins[i], OUTPUT);
  }

  // Tlačítko
  pinMode(buttonPin, INPUT_PULLUP);

  // Buzzer
  pinMode(buzzerPin, OUTPUT);

  // Start MPU6050
  if (!mpu.begin()) {

    Serial.println("MPU6050 nenalezen!");

    while (1) {
      delay(10);
    }
  }

  Serial.println("MPU6050 pripraven");
}

void loop() {

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Výpočet úhlu
  float angle = atan2(a.acceleration.y,
                      a.acceleration.z) * 180 / PI;

  // ===== TLAČÍTKO NOVÁ NULA =====
  bool buttonState = digitalRead(buttonPin);

  if(buttonState == LOW && lastButtonState == HIGH) {

    zeroAngle = angle;

    Serial.print("Nova nula nastavena: ");
    Serial.println(zeroAngle);

    // Potvrzovací pípnutí
    beep(100);
  }

  lastButtonState = buttonState;

  // Rozdíl od uložené roviny
  float diff = angle - zeroAngle;

  Serial.print("Odchylka: ");
  Serial.println(diff);

  // Zhasnout všechny LED
  for(int i = 0; i < 8; i++) {
    digitalWrite(ledPins[i], LOW);
  }

  // ===== ROVINA =====
  if(abs(diff) < 2) {

    // Rozsvítit obě prostřední LED
    digitalWrite(ledPins[3], HIGH);
    digitalWrite(ledPins[4], HIGH);

    // Přerušované pípání
    if(millis() - lastBeepTime > 500) {

      tone(buzzerPin, 1200, 80);

      lastBeepTime = millis();
    }

  } else {

    // Mapování úhlu na LED
    int ledIndex = map(diff, -45, 45, 7, 0);

    ledIndex = constrain(ledIndex, 0, 7);

    digitalWrite(ledPins[ledIndex], HIGH);
  }

  delay(50);
}