// FESB Racing - APPS + Pedal Mapping

// ── Pins ──────────────────────────
const int APPS1_PIN = A0;  // Sensor 1
const int APPS2_PIN = A1;  // Sensor 2
const int LED_PIN   = 13;  // LED
const int BTN_PIN   = 7;   // Button

// ── Timing ────────────────────────
const int APPS_INTERVAL    = 20;    // Sensoren alle 20ms lesen
const int DIAG_INTERVAL    = 500;   // UART Ausgabe alle 500ms
const int IMPLAUS_TIMEOUT  = 3000;  // FSG: 3 Sekunden Limit
const int BLINK_INTERVAL   = 200;   // LED blinkt alle 200ms
const int DEBOUNCE_MS      = 50;    // Button Debounce

// ── Zustand ───────────────────────
float apps1_pct = 0;    // Sensor 1 Wert in Prozent (0-100%)
float apps2_pct = 0;    // Sensor 2 Wert in Prozent (0-100%)
float throttle  = 0;    // Berechneter Gas-Wert

bool implausible     = false;  // Sind Sensoren zu weit auseinander?
bool inError         = false;  // Sind wir im Fehlerzustand?
bool mapIsLinear     = true;   // Welche Map ist aktiv?

unsigned long implausStart  = 0;  // Wann hat Fehler begonnen?
unsigned long lastAppsRead  = 0;  // Wann zuletzt Sensoren gelesen?
unsigned long lastDiag      = 0;  // Wann zuletzt UART ausgegeben?
unsigned long lastBlink     = 0;  // Wann zuletzt LED geblinkt?
unsigned long lastBtn       = 0;  // Wann zuletzt Button gedrückt?

// ── Setup ─────────────────────────
void setup() {
  Serial.begin(115200);
  pinMode(LED_PIN, OUTPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);
  Serial.println("FESB Racing APPS gestartet!");
}

// ── Hilfsfunktionen ───────────────
float normalisiere(int raw) {
  return (raw / 1023.0) * 100.0;
}

float mapLinear(float pct) {
  return pct;
}

float mapProgressiv(float pct) {
  return (pct * pct) / 100.0;
}

// ── Button ────────────────────────
void handleButton() {
  static bool letzterZustand = HIGH;
  if (millis() - lastBtn < DEBOUNCE_MS) return;
  bool jetzt = digitalRead(BTN_PIN);
  if (letzterZustand == HIGH && jetzt == LOW) {
    mapIsLinear = !mapIsLinear;
    Serial.println(mapIsLinear ? "Map: LINEAR" : "Map: PROGRESSIV");
    lastBtn = millis();
  }
  letzterZustand = jetzt;
}

// ── Fehler Handler ────────────────
void errorHandler() {
  inError = true;
  Serial.println("FEHLER! APPS Abweichung > 10% fuer 3 Sekunden!");
  while (true) {
    if (millis() - lastBlink >= BLINK_INTERVAL) {
      lastBlink = millis();
      digitalWrite(LED_PIN, !digitalRead(LED_PIN));
      Serial.println("FEHLER AKTIV - Gas = 0%");
    }
  }
}

// ── Sicherheitscheck ──────────────
void checkImplausibility() {
  float abweichung = abs(apps1_pct - apps2_pct);
  if (abweichung > 10.0) {
    if (!implausible) {
      implausible = true;
      implausStart = millis();
    } else if (millis() - implausStart >= IMPLAUS_TIMEOUT) {
      errorHandler();
    }
  } else {
    implausible = false;
  }
}

// ── Hauptschleife ─────────────────
void loop() {
  unsigned long now = millis();

  // Sensoren lesen
  if (now - lastAppsRead >= APPS_INTERVAL) {
    lastAppsRead = now;
    apps1_pct = normalisiere(analogRead(APPS1_PIN));
    apps2_pct = normalisiere(analogRead(APPS2_PIN));
    checkImplausibility();
    float avg = (apps1_pct + apps2_pct) / 2.0;
    throttle = mapIsLinear ? mapLinear(avg) : mapProgressiv(avg);
  }

  // Button prüfen
  handleButton();

  // LED anzeigen
  if (!inError) {
    digitalWrite(LED_PIN, mapIsLinear ? HIGH : LOW);
  }

  // Diagnose ausgeben
  if (now - lastDiag >= DIAG_INTERVAL) {
    lastDiag = now;
    Serial.print("APPS1: "); Serial.print(apps1_pct);
    Serial.print("% | APPS2: "); Serial.print(apps2_pct);
    Serial.print("% | Gas: "); Serial.print(throttle);
    Serial.print("% | Map: "); Serial.print(mapIsLinear ? "LINEAR" : "PROGRESSIV");
    Serial.print(" | Implausibel: "); Serial.println(implausible ? "JA" : "NEIN");
  }
}