/*
  4WD Robot Motor Control (2x BTS7960: Left + Right)
  - Soft start/stop via ramping (slew-rate limiter)
  - Differential drive mixing: throttle + turn
  - Serial control (USB): f/b/l/r/s or m <throttle> <turn>
  - Watchdog: stops if no command received in time

  Wiring (per BTS7960 module):
    - LPWM  -> Arduino PWM pin
    - RPWM  -> Arduino PWM pin
    - LEN/REN (or L_EN/R_EN) -> Arduino digital pins (or permanently HIGH to 5V)
    - VCC   -> 5V (logic)
    - GND   -> GND common with Arduino and motor supply
    - B+ / B- -> motor supply (e.g. 12V)
    - M+ / M- -> motor output

  NOTE:
    Each side is one BTS7960 module driving two motors in parallel (front+rear) on that side.
*/

#include <Arduino.h>
#include <PS2X_lib.h>
PS2X ps2x;

// ------ PS2 Controller Pins -------------
const uint8_t PS2_CLK = 2;
const uint8_t PS2_ATT = 3;
const uint8_t PS2_CMD = 4;
const uint8_t PS2_DAT = 12;

// ------- USE PS2 Controller or Serial Input --------------
#define USE_PS2 1

// ----------------------------- Pins (ANPASSEN) -----------------------------
// LEFT BTS7960
const uint8_t L_LPWM = 5;    // PWM pin
const uint8_t L_RPWM = 6;    // PWM pin
const uint8_t L_LEN  = 7;    // enable pin (LEN)
const uint8_t L_REN  = 8;    // enable pin (REN)

// RIGHT BTS7960
const uint8_t R_LPWM = 9;    // PWM pin
const uint8_t R_RPWM = 10;   // PWM pin
const uint8_t R_LEN  = 11;   // enable pin (LEN)
const uint8_t R_REN  = 12;   // enable pin (REN)

// --------------------------- Tuning / Limits -------------------------------
// PWM range: 0..255
const int PWM_MAX = 255;

// Ramp speed: PWM units per second (z.B. 300 => ~0.85s von 0 auf 255)
const float RAMP_UP_PER_SEC   = 320.0f;
const float RAMP_DOWN_PER_SEC = 520.0f; // meist darf bremsen/stoppen schneller sein

// Deadband: kleine Werte ignorieren (gegen "Zittern")
const int INPUT_DEADBAND = 10;

// Watchdog: wenn so lange kein Command kommt -> STOP
const uint32_t COMMAND_TIMEOUT_MS = 600;

// Loop interval (Ramping arbeitet zeitbasiert; häufig genug aufrufen)
const uint32_t CONTROL_INTERVAL_MS = 10;

// ---------------------------- State ----------------------------------------
volatile int targetLeft  = 0;   // -255..255
volatile int targetRight = 0;   // -255..255

float currentLeft  = 0.0f;      // ramped output -255..255
float currentRight = 0.0f;

uint32_t lastCmdMs = 0;
uint32_t lastControlMs = 0;

// ------------------------- Helpers -----------------------------------------
static int clampInt(int v, int lo, int hi) {
  if (v < lo) return lo;
  if (v > hi) return hi;
  return v;
}

static int applyDeadband(int v, int db) {
  if (abs(v) < db) return 0;
  return v;
}

// Ramp current towards target based on dt
static float rampTo(float current, float target, float dtSec) {
  float diff = target - current;
  if (diff == 0.0f) return current;

  const float rate = (abs(target) > abs(current)) ? RAMP_UP_PER_SEC : RAMP_DOWN_PER_SEC;
  float step = rate * dtSec;

  if (abs(diff) <= step) return target;
  return current + (diff > 0 ? step : -step);
}

// Send signed speed (-255..255) to one BTS7960
static void driveBTS7960(uint8_t lpwm, uint8_t rpwm, uint8_t len, uint8_t ren, int speed) {
  speed = clampInt(speed, -PWM_MAX, PWM_MAX);

  // Enable driver
  digitalWrite(len, HIGH);
  digitalWrite(ren, HIGH);

  int pwm = abs(speed);

  // IMPORTANT: never drive both PWM pins at the same time
  if (speed > 0) {
    analogWrite(lpwm, pwm);
    analogWrite(rpwm, 0);
  } else if (speed < 0) {
    analogWrite(lpwm, 0);
    analogWrite(rpwm, pwm);
  } else {
    analogWrite(lpwm, 0);
    analogWrite(rpwm, 0);
  }
}

static void stopAll() {
  targetLeft = 0;
  targetRight = 0;
}

// Differential drive mixing:
// throttle: -255..255, turn: -255..255
static void setMix(int throttle, int turn) {
  throttle = clampInt(throttle, -PWM_MAX, PWM_MAX);
  turn     = clampInt(turn,     -PWM_MAX, PWM_MAX);

  throttle = applyDeadband(throttle, INPUT_DEADBAND);
  turn     = applyDeadband(turn,     INPUT_DEADBAND);

  // Classic mix
  int left  = throttle + turn;
  int right = throttle - turn;

  // Normalize if exceeds range
  int maxMag = max(abs(left), abs(right));
  if (maxMag > PWM_MAX) {
    // scale down proportionally
    left  = (int)((float)left  * ((float)PWM_MAX / (float)maxMag));
    right = (int)((float)right * ((float)PWM_MAX / (float)maxMag));
  }

  targetLeft = left;
  targetRight = right;
}

// ------------------------- Serial command parser ---------------------------
// Commands:
//   f <pwm>   -> forward
//   b <pwm>   -> backward
//   l <pwm>   -> turn left (in place)
//   r <pwm>   -> turn right (in place)
//   s         -> stop
//   m <throttle> <turn>  -> mix mode (-255..255 each)
// Examples:
//   f 140
//   m 120 -40
//   s
static void handleSerialLine(String line) {
  line.trim();
  if (line.length() == 0) return;

  char cmd = tolower(line.charAt(0));

  // Update watchdog timestamp on any valid-looking input
  lastCmdMs = millis();

  if (cmd == 's') {
    stopAll();
    return;
  }

  // Split by spaces
  // Simple parsing:
  // cmd + integers
  int a = 0, b = 0;
  int n = 0;

  // Try parse "m a b"
  if (cmd == 'm') {
    n = sscanf(line.c_str(), "m %d %d", &a, &b);
    if (n == 2) {
      setMix(a, b);
    }
    return;
  }

  // Parse "f a" / "b a" / "l a" / "r a"
  n = sscanf(line.c_str(), "%c %d", &cmd, &a);
  if (n < 2) return;

  a = clampInt(a, 0, PWM_MAX);

  switch (tolower(cmd)) {
    case 'f': setMix(+a, 0); break;
    case 'b': setMix(-a, 0); break;
    case 'l': setMix(0, +a); break; // left turn in place
    case 'r': setMix(0, -a); break; // right turn in place
    default: break;
  }
}

// ------------------------------ Arduino ------------------------------------
void setup() {
  int err = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_ATT, PS2_DAT, false, false);
  Serial.print("PS2 init: "); Serial.println(err);

  pinMode(L_LPWM, OUTPUT);
  pinMode(L_RPWM, OUTPUT);
  pinMode(L_LEN, OUTPUT);
  pinMode(L_REN, OUTPUT);

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

  // Init: disabled PWM = 0, enables on
  analogWrite(L_LPWM, 0); analogWrite(L_RPWM, 0);
  analogWrite(R_LPWM, 0); analogWrite(R_RPWM, 0);

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

  Serial.begin(115200);
  Serial.println(F("BTS7960 Robot Control ready."));
  Serial.println(F("Commands: f/b/l/r <0..255>, m <throttle -255..255> <turn -255..255>, s"));

  lastCmdMs = millis();
  lastControlMs = millis();
}

void loop() {
  #if USE_PS2
    // -------- PS2 Controller Input -------
    ps2x.read_gamepad(false, 0);

    // Not-Stop z.B. START
    if (ps2x.ButtonPressed(PSB_START)) {
      stopAll();
      lastCmdMs = millis();     // watchdog “füttern”
    }

    // Sticks lesen
    int ly = ps2x.Analog(PSS_LY);   // 0..255
    int rx = ps2x.Analog(PSS_RX);   // 0..255

    // 0..255 -> -255..255
    auto stickToSigned = [](int v) {
      int x = (v - 128) * 2;
      if (x > 255) x = 255;
      if (x < -255) x = -255;
      return x;
    };

    // Mapping: LY nach oben = vorwärts (invertieren)
    int throttle = -stickToSigned(ly);
    int turn     =  stickToSigned(rx);

    lastCmdMs = millis();       // watchdog “füttern”
    setMix(throttle, turn);     // das ist dein Original-Mixer

  #else
  // -------- Serial input (line based) --------
  static String buf;
  while (Serial.available()) {
    char c = (char)Serial.read();
    if (c == '\n' || c == '\r') {
      if (buf.length() > 0) {
        handleSerialLine(buf);
        buf = "";
      }
    } else {
      // avoid huge line
      if (buf.length() < 80) buf += c;
    }
  }
  #ENDIF

  // -------- Watchdog --------
  if (millis() - lastCmdMs > COMMAND_TIMEOUT_MS) {
    stopAll();
  }

  // -------- Control update (ramping + output) --------
  uint32_t now = millis();
  if (now - lastControlMs >= CONTROL_INTERVAL_MS) {
    float dt = (now - lastControlMs) / 1000.0f;
    lastControlMs = now;

    // ramp current values towards target
    currentLeft  = rampTo(currentLeft,  (float)targetLeft,  dt);
    currentRight = rampTo(currentRight, (float)targetRight, dt);

    // apply to drivers
    driveBTS7960(L_LPWM, L_RPWM, L_LEN, L_REN, (int)round(currentLeft));
    driveBTS7960(R_LPWM, R_RPWM, R_LEN, R_REN, (int)round(currentRight));
  }
}