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

// --- DEINE HARDWARE PINS ---
#define STEP_PIN   PA8    // D9 am Nucleo
#define DIR_PIN    PA9    // D8 am Nucleo

// Sensor Instanz
Adafruit_MPU6050 mpu;

// --- PID PARAMETER FÜR 50% REDUKTION ---
float Kp = 50.0;   // Proportionaler Anteil (Reaktionsstärke)
float Ki = 2.0;    // Integraler Anteil (Drift-Korrektur)
float Kd = 12.0;   // Differenzieller Anteil (Dämpfung - verhindert Schwingen)

float target = 0.0; // Zielwert: 0 m/s² Vibration
float integral = 0, last_error = 0;
unsigned long last_time;

void setup() {
  Serial.begin(115200);

  // Initialisierung deiner Pins
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(DIR_PIN, HIGH);

  // I2C Konfiguration für STM32 Nucleo-C031C6 (SDA=PA10, SCL=PA11)
  Wire.setSDA(PA10);
  Wire.setSCL(PA11);
  Wire.begin();

  // Sensor-Check
  if (!mpu.begin()) {
    while (1) delay(10);
  }

  // Filterung für saubere Messwerte (Vibrationen bis 5Hz)
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  last_time = micros();
}

void loop() {
  // 1. SENSORWERTE ERFASSEN
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Zeitdifferenz für präzise PID-Berechnung
  unsigned long now = micros();
  float dt = (now - last_time) / 1000000.0;
  if (dt <= 0) dt = 0.001; 
  last_time = now;

  // 2. FEHLERBERECHNUNG (Ist-Wert vs. Ziel-Wert)
  // Z-Achse Beschleunigung minus Erdgravitation
  float current_accel = a.acceleration.z - 9.81; 
  float error = target - current_accel;

  // 3. PID-ALGORITHMUS
  integral += error * dt;
  float derivative = (error - last_error) / dt;
  float output = (Kp * error) + (Ki * integral) + (Kd * derivative);
  last_error = error;

  // 4. DEINE MOTORSTEUERUNG (AKTIV GEREGELT)
  // Nur reagieren, wenn die Vibration einen Schwellenwert überschreitet
  if (abs(output) > 0.1) {
    
    // Richtung basierend auf PID-Ergebnis setzen
    digitalWrite(DIR_PIN, (output > 0) ? HIGH : LOW);

    // Ein Korrekturschritt (Deine Timing-Logik: 800µs)
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(800); 
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(800);
  }

  // 5. PLOTTER-AUSGABE (Beweis der Kompensation für die Präsentation)
  Serial.print("Vibration:"); Serial.print(current_accel);
  Serial.print("Kompensationskraft:"); Serial.println(output / 10);
}