/*
  Single BTS7960 test on Arduino UNO
  Pins:
    D5 -> LPWM
    D6 -> RPWM
    D7 -> LEN
    D8 -> REN

  Test:
    - automatically: forward -> stop -> backward -> stop (loop)
    - optional serial commands:
        f <0..255>
        b <0..255>
        s
*/

#include <Arduino.h>

const uint8_t LPWM = 5;   // PWM
const uint8_t RPWM = 6;   // PWM
const uint8_t LEN  = 7;   // Enable
const uint8_t REN  = 8;   // Enable

const int PWM_MAX = 255;

// Ramp tuning (PWM steps per second)
const float RAMP_UP_PER_SEC   = 320.0f;
const float RAMP_DOWN_PER_SEC = 520.0f;

const uint32_t CONTROL_INTERVAL_MS = 10;

int target = 0;        // -255..255
float current = 0.0f;  // ramped output
uint32_t lastControlMs = 0;

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

static float rampTo(float cur, float tgt, float dtSec) {
  float diff = tgt - cur;
  if (diff == 0.0f) return cur;

  float rate = (abs(tgt) > abs(cur)) ? RAMP_UP_PER_SEC : RAMP_DOWN_PER_SEC;
  float step = rate * dtSec;

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

static void driveBTS(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 >0 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 setTarget(int speed) {
  target = clampInt(speed, -PWM_MAX, PWM_MAX);
}

static void stopMotor() {
  setTarget(0);
}

static void handleSerialLine(String line) {
  line.trim();
  if (line.length() == 0) return;

  char c = tolower(line.charAt(0));
  int v = 0;

  if (c == 's') {
    stopMotor();
    Serial.println(F("STOP"));
    return;
  }

  if (sscanf(line.c_str(), "%c %d", &c, &v) < 2) return;
  v = clampInt(v, 0, PWM_MAX);

  if (c == 'f') {
    setTarget(+v);
    Serial.print(F("FWD ")); Serial.println(v);
  } else if (c == 'b') {
    setTarget(-v);
    Serial.print(F("BWD ")); Serial.println(v);
  }
}

void setup() {
  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LEN, OUTPUT);
  pinMode(REN, OUTPUT);

  analogWrite(LPWM, 0);
  analogWrite(RPWM, 0);
  digitalWrite(LEN, HIGH);
  digitalWrite(REN, HIGH);

  Serial.begin(115200);
  Serial.println(F("BTS7960 single-driver test ready."));
  Serial.println(F("Serial: f <0..255>, b <0..255>, s"));
  lastControlMs = millis();
}

void loop() {
  // --- Serial line input ---
  static String buf;
  while (Serial.available()) {
    char ch = (char)Serial.read();
    if (ch == '\n' || ch == '\r') {
      if (buf.length() > 0) {
        handleSerialLine(buf);
        buf = "";
      }
    } else {
      if (buf.length() < 60) buf += ch;
    }
  }

  // --- Automatic test sequence (only if no serial commands used) ---
  // Comment this block out if you want ONLY serial control.
  static uint32_t t0 = millis();
  static int phase = 0;

  uint32_t now = millis();
  if (now - t0 > 2000) { // change every 2s
    t0 = now;
    phase = (phase + 1) % 4;

    if (phase == 0) setTarget(+80);   // forward slow
    if (phase == 1) stopMotor();      // stop
    if (phase == 2) setTarget(-80);   // backward slow
    if (phase == 3) stopMotor();      // stop
  }

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

    current = rampTo(current, (float)target, dt);
    driveBTS((int)lround(current));
  }
}