#include <Servo.h>

// ===== Motor 1 (DRV8421 — chip A) =====
#define A1_AIN1 29
#define A1_AIN2 28
#define A1_BIN1 27
#define A1_BIN2 26

// ===== Motor 2 (DRV8421 — chip B) =====
#define A2_AIN1 3
#define A2_AIN2 4
#define A2_BIN1 2
#define A2_BIN2 1

// ===== Servo =====
#define SERVO_PIN 0
Servo myServo;

const int limitPWM = 143;
const unsigned long stepDelayUs = 3000;
const unsigned long servoHoldMs = 300;

int phase = 0;
unsigned long lastStepTime = 0;
unsigned long totalSteps = 0;

enum ServoState { SERVO_IDLE, SERVO_WAIT_AT_90, SERVO_WAIT_AT_0 };
ServoState servoState = SERVO_IDLE;
unsigned long servoTimestamp = 0;

void setup() {
  pinMode(A1_AIN1, OUTPUT); pinMode(A1_AIN2, OUTPUT);
  pinMode(A1_BIN1, OUTPUT); pinMode(A1_BIN2, OUTPUT);
  pinMode(A2_AIN1, OUTPUT); pinMode(A2_AIN2, OUTPUT);
  pinMode(A2_BIN1, OUTPUT); pinMode(A2_BIN2, OUTPUT);

  myServo.attach(SERVO_PIN);
  myServo.write(0);
}

void writeCoil(int pin, bool on) {
  if (on) analogWrite(pin, limitPWM);
  else    digitalWrite(pin, LOW);
}

void setStepBoth(bool a1, bool a2, bool b1, bool b2) {
  writeCoil(A1_AIN1, a1); writeCoil(A1_AIN2, a2);
  writeCoil(A1_BIN1, b1); writeCoil(A1_BIN2, b2);
  writeCoil(A2_AIN1, a1); writeCoil(A2_AIN2, a2);
  writeCoil(A2_BIN1, b1); writeCoil(A2_BIN2, b2);
}

void stepForwardOnce() {
  switch (phase) {
    case 0: setStepBoth(true,  false, true,  false); break;
    case 1: setStepBoth(false, true,  true,  false); break;
    case 2: setStepBoth(false, true,  false, true);  break;
    case 3: setStepBoth(true,  false, false, true);  break;
  }
  phase = (phase + 1) % 4;
  totalSteps++;

  if (totalSteps % 3 == 0) startServoAction();
}

void startServoAction() {
  if (servoState == SERVO_IDLE) {
    myServo.write(90);
    servoTimestamp = millis();
    servoState = SERVO_WAIT_AT_90;
  }
}

void updateServo() {
  unsigned long now = millis();
  switch (servoState) {
    case SERVO_IDLE: break;
    case SERVO_WAIT_AT_90:
      if (now - servoTimestamp >= servoHoldMs) {
        myServo.write(0);
        servoTimestamp = now;
        servoState = SERVO_WAIT_AT_0;
      }
      break;
    case SERVO_WAIT_AT_0:
      if (now - servoTimestamp >= servoHoldMs) {
        servoState = SERVO_IDLE;
      }
      break;
  }
}

void updateStepper() {
  unsigned long now = micros();
  if (now - lastStepTime >= stepDelayUs) {
    lastStepTime = now;
    stepForwardOnce();
  }
}

void loop() {
  updateStepper();
  updateServo();
}