/*
  Arduino Drive "Safe Executor"
  - Receives:  L <int> R <int>   (-255..255)
  - Outputs PWM immediately (no ramping here)
  - Safety: watchdog stop if no command
  - Deadzone + Min PWM mapping to avoid motor buzzing at low PWM
  - Optional direction-change protection (short stop)
*/

// ===== Left BTS7960 pins =====
const uint8_t L_RPWM = 5;
const uint8_t L_LPWM = 6;
const uint8_t L_REN  = 7;
const uint8_t L_LEN  = 8;

// ===== Right BTS7960 pins =====
const uint8_t R_RPWM = 9;
const uint8_t R_LPWM = 10;
const uint8_t R_REN  = 11;
const uint8_t R_LEN  = 12;

// ===== Safety / feel =====
const uint16_t CMD_TIMEOUT_MS = 600;  // Web sendet regelmäßig; 600ms ist entspannt
const uint8_t  DEADZONE = 10;         // kleine Werte -> 0
const uint8_t  MIN_PWM  = 70;         // anpassen: 60..110 typisch (gegen "brummen")

// Optional: beim Richtungswechsel kurz stoppen (schont Treiber/Getriebe)
const bool     PROTECT_DIR_CHANGE = true;
const uint16_t DIR_CHANGE_STOP_MS = 60;

int targetL = 0, targetR = 0;
unsigned long lastCmdMs = 0;

int lastOutL = 0, lastOutR = 0;

static int clamp255(int v) {
  if (v > 255) return 255;
  if (v < -255) return -255;
  return v;
}

int applyMinPwm(int v) {
  v = clamp255(v);
  int a = abs(v);
  if (a <= DEADZONE) return 0;

  int s = (v >= 0) ? 1 : -1;

  // map: DEADZONE..255 -> MIN_PWM..255
  long mapped = MIN_PWM + (long)(a - DEADZONE) * (255 - MIN_PWM) / (255 - DEADZONE);
  if (mapped > 255) mapped = 255;

  return s * (int)mapped;
}

void setBTS7960(int speed, uint8_t rpwm, uint8_t lpwm) {
  speed = clamp255(speed);
  if (speed > 0) {
    analogWrite(rpwm, (uint8_t)speed);
    analogWrite(lpwm, 0);
  } else if (speed < 0) {
    analogWrite(rpwm, 0);
    analogWrite(lpwm, (uint8_t)(-speed));
  } else {
    analogWrite(rpwm, 0);
    analogWrite(lpwm, 0);
  }
}

bool parseLine(const String& line, int &outL, int &outR) {
  int idxL = line.indexOf('L');
  int idxR = line.indexOf('R');
  if (idxL < 0 || idxR < 0) return false;

  String partL = line.substring(idxL + 1, idxR);
  String partR = line.substring(idxR + 1);
  partL.trim(); partR.trim();

  outL = clamp255(partL.toInt());
  outR = clamp255(partR.toInt());
  return true;
}

void setupPins() {
  pinMode(L_RPWM, OUTPUT); pinMode(L_LPWM, OUTPUT);
  pinMode(L_REN, OUTPUT);  pinMode(L_LEN, OUTPUT);

  pinMode(R_RPWM, OUTPUT); pinMode(R_LPWM, OUTPUT);
  pinMode(R_REN, OUTPUT);  pinMode(R_LEN, OUTPUT);

  digitalWrite(L_REN, HIGH); digitalWrite(L_LEN, HIGH);
  digitalWrite(R_REN, HIGH); digitalWrite(R_LEN, HIGH);

  setBTS7960(0, L_RPWM, L_LPWM);
  setBTS7960(0, R_RPWM, R_LPWM);
}

void outputLR(int l, int r) {
  l = applyMinPwm(l);
  r = applyMinPwm(r);

  if (PROTECT_DIR_CHANGE) {
    // if sign changes across 0 while moving -> brief stop
    auto sign = [](int v) -> int { return (v > 0) - (v < 0); };

    bool lFlip = (sign(lastOutL) != 0) && (sign(l) != 0) && (sign(lastOutL) != sign(l));
    bool rFlip = (sign(lastOutR) != 0) && (sign(r) != 0) && (sign(lastOutR) != sign(r));

    if (lFlip || rFlip) {
      setBTS7960(0, L_RPWM, L_LPWM);
      setBTS7960(0, R_RPWM, R_LPWM);
      delay(DIR_CHANGE_STOP_MS);
    }
  }

  setBTS7960(l, L_RPWM, L_LPWM);
  setBTS7960(r, R_RPWM, R_LPWM);

  lastOutL = l;
  lastOutR = r;
}

void setup() {
  Serial.begin(115200);
  setupPins();
  delay(200);

  Serial.println(F("Drive Ready. Send: L <val> R <val>  (-255..255)"));
  Serial.print(F("DEADZONE=")); Serial.print(DEADZONE);
  Serial.print(F(" MIN_PWM=")); Serial.println(MIN_PWM);

  lastCmdMs = millis();
}

void loop() {
  while (Serial.available()) {
    String line = Serial.readStringUntil('\n');
    line.trim();
    if (line.length() == 0) continue;

    int l, r;
    if (parseLine(line, l, r)) {
      targetL = l;
      targetR = r;
      lastCmdMs = millis();

      outputLR(targetL, targetR);

      Serial.print(F("RX/OUT L=")); Serial.print(lastOutL);
      Serial.print(F(" R=")); Serial.println(lastOutR);
    } else {
      Serial.println(F("Parse error -> STOP"));
      targetL = 0; targetR = 0;
      lastCmdMs = millis();
      outputLR(0, 0);
    }
  }

  // Watchdog stop
   if (millis() - lastCmdMs > CMD_TIMEOUT_MS) {
    outputLR(0, 0);
  }
}