#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7735.h>
#include <MPU6050.h>
#include <math.h>

#define TFT_CS 10
#define TFT_RST 8
#define TFT_DC 9
#define BUZZER_PIN 2
#define CALIB_BUTTON_PIN 3

Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);
MPU6050 mpu;

const float MAX_ROLL = 30.0;
const float MAX_PITCH = 40.0;
const float WARN_ROLL = 25.0;
const float WARN_PITCH = 35.0;

// Offsets für sichtbare Pixel
const int V_OFFSET = 3;
const int H_OFFSET = 4;
// Gefilterte Sensorwerte
float rollFiltered = 0;
float pitchFiltered = 0;
// Kalibrierungs-Offsets
float rollOffset = 0;
float pitchOffset = 0;
// Subpixel-Balkenwerte
float rollBar = 0, pitchBar = 0;
float targetRollBar = 0, targetPitchBar = 0;
// Subpixel-Textwerte
float displayRollText = 0, displayPitchText = 0;
float targetRollText = 0, targetPitchText = 0;
// Filter & Animation
const float alpha = 0.2;
const float barAlpha = 0.3;
const float textAlpha = 0.3;
const int BAR_HEIGHT = 20;
// Timing
unsigned long lastUpdate = 0;
const int UPDATE_INTERVAL = 5; // ca. 200 FPS
// Blink-Variablen für Warnung
bool blinkState = false;
unsigned long lastBlink = 0;
const int BLINK_INTERVAL = 500; // 0,5 Sekunden
// Header Text
String headerText = "Neigung";
// Float-taugliche map-Funktion
float fmap(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setup() {
  Serial.begin(9600);
  Wire.begin();
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(CALIB_BUTTON_PIN, INPUT_PULLUP);
  // tft.setRotation(0);
  tft.initR(INITR_BLACKTAB);
  tft.fillScreen(ST77XX_BLACK);
  // Header
  tft.setTextSize(2);
  int16_t headerX1, headerY1;
  uint16_t headerW, headerH;
  tft.getTextBounds(headerText, 0, 0, &headerX1, &headerY1, &headerW, &headerH);
  int headerX = (tft.width() - headerW) / 2;
  int headerY = V_OFFSET;
  tft.setCursor(headerX, headerY);
  tft.setTextColor(ST77XX_YELLOW);
  tft.println(headerText);
  mpu.initialize();
  delay(100);
  Serial.println(mpu.testConnection() ? "MPU6050 verbunden!" : "MPU6050 Fehler!");
  // Rahmen für Balken
  tft.drawRect(H_OFFSET, 30 + V_OFFSET, tft.width() - 2 * H_OFFSET, BAR_HEIGHT, ST77XX_WHITE); // Roll
  tft.drawRect(H_OFFSET, 80 + V_OFFSET, tft.width() - 2 * H_OFFSET, BAR_HEIGHT, ST77XX_WHITE); // Pitch
  // Labels
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW);
  tft.setCursor(H_OFFSET, 55 + V_OFFSET);
  tft.print("Roll: ");
  tft.setCursor(H_OFFSET, 105 + V_OFFSET);
  tft.print("Pitch: ");
}

void loop() {
  unsigned long now = millis();
  if (now - lastUpdate < UPDATE_INTERVAL) return;
  lastUpdate = now;
  // ----------------------------
  // Button Handling (Kalibrierung / Reset)
  // ----------------------------
  static unsigned long buttonPressTime = 0;
  static bool buttonState = HIGH;
  static bool lastButtonReading = HIGH;
  static unsigned long lastDebounceTime = 0;
  const unsigned long debounceDelay = 50;
  static String feedbackText = "";
  static unsigned long feedbackStart = 0;
  bool reading = digitalRead(CALIB_BUTTON_PIN);
  // Entprellung
  if (reading != lastButtonReading) {
    lastDebounceTime = millis();
  }
  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == LOW) {
        buttonPressTime = millis();
        tone(BUZZER_PIN, 2000, 100);
      } else {
        unsigned long pressDuration = millis() - buttonPressTime;
        Serial.print("Tastendruckdauer: ");
        Serial.println(pressDuration);
        if (pressDuration < 800) {
          rollOffset = rollFiltered;
          pitchOffset = pitchFiltered;
          feedbackText = "Kalibriert!";
          feedbackStart = millis();
          Serial.println("Sensor kalibriert!");
          tone(BUZZER_PIN, 1500, 150);
        } else if (pressDuration >= 2500) {
          rollOffset = 0;
          pitchOffset = 0;
          feedbackText = "Reset!";
          feedbackStart = millis();
          Serial.println("Offsets auf Werkseinstellungen zurückgesetzt!");
          tone(BUZZER_PIN, 500, 400);
        } else {
          feedbackText = "Zu lang/kurz!";
          feedbackStart = millis();
          Serial.println("Ungültige Druckdauer!");
          tone(BUZZER_PIN, 800, 200);
        }
      }
    }
  }
  lastButtonReading = reading;
  if (feedbackText != "" && millis() - feedbackStart < 1000) {
    int warnHeight = 20;
    int16_t x1, y1;
    uint16_t w, h;
    tft.getTextBounds(feedbackText, 0, 0, &x1, &y1, &w, &h);
    int textX = (tft.width() - w) / 2;
    int textY = 130 + V_OFFSET + (warnHeight - h) / 2;
    tft.fillRect(0, 130 + V_OFFSET, tft.width(), warnHeight, ST77XX_BLACK);
    tft.setTextColor(ST77XX_YELLOW);
    tft.setTextSize(2);
    tft.setCursor(textX, textY);
    tft.println(feedbackText);
  } else if (millis() - feedbackStart >= 1000) {
    feedbackText = "";
    tft.fillRect(0, 130 + V_OFFSET, tft.width(), 20, ST77XX_BLACK);
  }
  // ----------------------------
  // MPU6050 Werte auslesen
  // ----------------------------
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  float ax_f = ax / 16384.0;
  float ay_f = ay / 16384.0;
  float az_f = az / 16384.0;
  // ----- 90° Roll-Drehung kompensieren -----
  float adjusted_ax = ax_f;   // X bleibt X
  float adjusted_ay = -az_f;  // neue Y
  float adjusted_az = ay_f;   // neue Z
  // ACHSENTAUSCH nach Roll-Drehung
  float pitch = atan2(adjusted_ay, sqrt(adjusted_ax * adjusted_ax + adjusted_az * adjusted_az)) * 180.0 / PI;
  float roll  = atan2(adjusted_ax, sqrt(adjusted_ay * adjusted_ay + adjusted_az * adjusted_az)) * 180.0 / PI;
  pitch = constrain(pitch, -MAX_PITCH, MAX_PITCH);
  roll  = constrain(roll, -MAX_ROLL, MAX_ROLL);
  rollFiltered  = rollFiltered * (1 - alpha) + roll * alpha;
  pitchFiltered = pitchFiltered * (1 - alpha) + pitch * alpha;
  float rollCalibrated = rollFiltered - rollOffset;
  float pitchCalibrated = pitchFiltered - pitchOffset;
  int frameWidth = tft.width() - 2 * H_OFFSET;
  int mid = frameWidth / 2;
  targetRollBar  = fmap(rollCalibrated, -MAX_ROLL, MAX_ROLL, -mid, mid);
  targetPitchBar = fmap(pitchCalibrated, -MAX_PITCH, MAX_PITCH, -mid, mid);
  targetRollText  = rollCalibrated;
  targetPitchText = pitchCalibrated;
  rollBar  = rollBar * (1 - barAlpha) + targetRollBar * barAlpha;
  pitchBar = pitchBar * (1 - barAlpha) + targetPitchBar * barAlpha;
  float diffRoll = targetRollText - displayRollText;
  float diffPitch = targetPitchText - displayPitchText;
  if (abs(diffRoll) > 5.0) displayRollText = targetRollText;
  else displayRollText = displayRollText * (1 - textAlpha) + targetRollText * textAlpha;
  if (abs(diffPitch) > 5.0) displayPitchText = targetPitchText;
  else displayPitchText = displayPitchText * (1 - textAlpha) + targetPitchText * textAlpha;
  bool warning = (abs(rollCalibrated) >= WARN_ROLL) || (abs(pitchCalibrated) >= WARN_PITCH);
  if (millis() - lastBlink > BLINK_INTERVAL) {
    lastBlink = millis();
    blinkState = !blinkState;
  }
  if (warning) {
    unsigned long t = millis();
    if ((t / 200) % 2 == 0) tone(BUZZER_PIN, 3000);
    else noTone(BUZZER_PIN);
  } else noTone(BUZZER_PIN);
  drawSmoothBar(30 + V_OFFSET, rollBar, (abs(rollCalibrated) >= WARN_ROLL) ? ST77XX_RED : ST77XX_GREEN);
  drawSmoothBar(80 + V_OFFSET, pitchBar, (abs(pitchCalibrated) >= WARN_PITCH) ? ST77XX_RED : ST77XX_BLUE);
  drawSmoothNumber(55 + V_OFFSET, displayRollText);
  drawSmoothNumber(105 + V_OFFSET, displayPitchText);
  int warnHeight = 20;
  String warnText = "NEIGUNG!";
  tft.setTextSize(2);
  int16_t warnX1, warnY1;
  uint16_t warnW, warnH;
  tft.getTextBounds(warnText, 0, 0, &warnX1, &warnY1, &warnW, &warnH);
  int warnY = 130 + V_OFFSET;
  int textY = warnY + (warnHeight - warnH) / 2;
  int textX = (tft.width() - warnW) / 2;
  if (warning && blinkState) {
    tft.fillRect(0, warnY, tft.width(), warnHeight, ST77XX_RED);
    tft.setTextColor(ST77XX_YELLOW);
    for (int dx = 0; dx <= 1; dx++) {
      for (int dy = 0; dy <= 1; dy++) {
        tft.setCursor(textX + dx, textY + dy);
        tft.println(warnText);
      }
    }
  } else {
    tft.fillRect(0, warnY, tft.width(), warnHeight, ST77XX_BLACK);
  }
}
// Balken zeichnen
void drawSmoothBar(int y, float length, uint16_t color) {
  static float oldLengthR[2] = {0, 0};
  int frameWidth = tft.width() - 2 * H_OFFSET;
  int mid = H_OFFSET + frameWidth / 2;
  int oldInt = round(oldLengthR[y > 50 ? 1 : 0]);
  int newInt = round(length);
  if (oldInt > 0) tft.fillRect(mid, y, oldInt, BAR_HEIGHT, ST77XX_BLACK);
  else tft.fillRect(mid + oldInt, y, -oldInt, BAR_HEIGHT, ST77XX_BLACK);
  if (newInt > 0) tft.fillRect(mid, y, newInt, BAR_HEIGHT, color);
  else tft.fillRect(mid + newInt, y, -newInt, BAR_HEIGHT, color);
  oldLengthR[y > 50 ? 1 : 0] = length;
  tft.drawRect(H_OFFSET, y, frameWidth, BAR_HEIGHT, ST77XX_WHITE);
}
// Zahlenanzeige
void drawSmoothNumber(int y, float value) {
  static float oldRollText = 0;
  static float oldPitchText = 0;
  float* oldValue = (y < 60) ? &oldRollText : &oldPitchText;
  int textX = H_OFFSET + 60;
  int textWidth = 60;
  int textHeight = 16;
  if (abs(value - *oldValue) > 0.01) {
    tft.fillRect(textX, y, textWidth, textHeight, ST77XX_BLACK);
    tft.setCursor(textX, y);
    tft.setTextSize(2);
    tft.setTextColor(ST77XX_YELLOW);
    tft.println(value, 1);
    *oldValue = value;
  }
}