// ===== Arduino UNO: Differential Drive über 2x BTS7960 =====
// Serial command format: "L <int> R <int>\n"  where int in [-255..255]
// Soft-start ramping + watchdog stop

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

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

// Ramping
const uint8_t RAMP_STEP = 6;        // speed change per loop (0..255)
const uint16_t LOOP_MS  = 15;       // ramp update interval
const uint16_t CMD_TIMEOUT_MS = 300; // stop if no command within x ms

int targetL = 0, targetR = 0;
int currentL = 0, currentR = 0;

unsigned long lastCmdMs = 0;
unsigned long lastLoopMs = 0;

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);

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

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

void driveOne(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);
  }
}

static int rampTowards(int current, int target, uint8_t step) {
  if (current < target) {
    int n = current + step;
    return (n > target) ? target : n;
  }
  if (current > target) {
    int n = current - step;
    return (n < target) ? target : n;
  }
  return current;
}

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

  // crude parse; robust enough for our simple format
  String partL = line.substring(idxL + 1, idxR);
  String partR = line.substring(idxR + 1);

  partL.trim();
  partR.trim();

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

void setup() {
  Serial.begin(115200);
  setupPins();
  lastCmdMs = millis();
  lastLoopMs = millis();
}

void loop() {
  // --- read serial lines ---
  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();
    }
  }

  // --- watchdog stop ---
  if (millis() - lastCmdMs > CMD_TIMEOUT_MS) {
    targetL = 0;
    targetR = 0;
  }

  // --- ramp update ---
  if (millis() - lastLoopMs >= LOOP_MS) {
    lastLoopMs = millis();

    currentL = rampTowards(currentL, targetL, RAMP_STEP);
    currentR = rampTowards(currentR, targetR, RAMP_STEP);

    driveOne(currentL, L_RPWM, L_LPWM);
    driveOne(currentR, R_RPWM, R_LPWM);
  }
}