// Pin definitions for XIAO RP2040
#define AIN1_PIN 29  // DRV8421A IN1
#define AIN2_PIN 28  // DRV8421A IN2
#define BIN1_PIN 27  // DRV8421A IN3
#define BIN2_PIN 26  // DRV8421A IN4

// PWM value to limit effective voltage to ~2.8 V with a 5.0 V supply
// Calculation: 255 * (2.8 / 5.0) ≈ 143
const int limitPWM = 143;

// Step period in microseconds (lower = faster)
int stepDelayUs = 3000;

// NEMA17: 1.8° per full step → 200 steps per revolution
const int stepsPerRev = 200;

void setup() {
  pinMode(AIN1_PIN, OUTPUT);
  pinMode(AIN2_PIN, OUTPUT);
  pinMode(BIN1_PIN, OUTPUT);
  pinMode(BIN2_PIN, OUTPUT);
}

/**
 * Drives the four H-bridge inputs for one step.
 * Uses analogWrite for active phases to PWM-limit motor current.
 */
void setStep(bool a1, bool a2, bool b1, bool b2) {
  if (a1) analogWrite(AIN1_PIN, limitPWM); else digitalWrite(AIN1_PIN, LOW);
  if (a2) analogWrite(AIN2_PIN, limitPWM); else digitalWrite(AIN2_PIN, LOW);
  if (b1) analogWrite(BIN1_PIN, limitPWM); else digitalWrite(BIN1_PIN, LOW);
  if (b2) analogWrite(BIN2_PIN, limitPWM); else digitalWrite(BIN2_PIN, LOW);
  delayMicroseconds(stepDelayUs);
}

// Full-step sequence — Figure 7-4 in the DRV8421 datasheet
void moveForward() {
  for (int i = 0; i < stepsPerRev; i++) {
    switch (i % 4) {
      case 0: setStep(true,  false, true,  false); break;
      case 1: setStep(false, true,  true,  false); break;
      case 2: setStep(false, true,  false, true);  break;
      case 3: setStep(true,  false, false, true);  break;
    }
  }
}

// Reverse of the full-step sequence
void moveBackward() {
  for (int i = 0; i < stepsPerRev; i++) {
    switch (i % 4) {
      case 0: setStep(true,  false, false, true);  break;
      case 1: setStep(false, true,  false, true);  break;
      case 2: setStep(false, true,  true,  false); break;
      case 3: setStep(true,  false, true,  false); break;
    }
  }
}

void loop() {
  moveForward();
  delay(1000);
  moveBackward();
  delay(1000);
}