// --- Hardware Configuration ---
// Pins connected to the ULN2003 driver (or similar stepper driver)
#define IN1 9
#define IN2 10
#define IN3 11
#define IN4 12

// Step sequences for 28BYJ-48
// Clockwise sequence
int stepSequence[4][4] = {
  { 1, 0, 0, 0 },
  { 0, 1, 0, 0 },
  { 0, 0, 1, 0 },
  { 0, 0, 0, 1 }
};

// --- Configuration Constants ---
const int STEPS_PER_REVOLUTION = 2048; // 28BYJ-48 standard (512 steps * 4 gear reduction)
const float DEGREES_PER_STEP = 360.0 / STEPS_PER_REVOLUTION;

// --- Global Variables ---
enum State { MOVE, WAIT };
enum Mode { AUTO, MANUAL };

Mode autoMode = AUTO;          // Default: AUTO
float targetAngle = 0.0;       // Target angle in degrees
float turnSpeed = 5.0;         // Speed in degrees per second

State _currState = MOVE;       // Default: MOVE

float waitInterval = 8.0;      // Wait time in seconds (default 8s)
unsigned long _waitTime = 0;   // Timestamp for wait state

float _stepInterval = 0.0;     // Time between steps in seconds
unsigned long _stepTime = 0;   // Timestamp for last step

int _stepCount = 0;            // Total steps to move
int _stepsRemaining = 0;       // Steps left in current move
int _currentStepIndex = 0;     // Current index in step sequence (0-3)
bool _directionCW = true;      // true = CW, false = CCW

// --- Helper Functions ---

void setStep(int a, int b, int c, int d) {
  digitalWrite(IN1, a);
  digitalWrite(IN2, b);
  digitalWrite(IN3, c);
  digitalWrite(IN4, d);
}

void updateStepInterval() {
  if (turnSpeed <= 0) {
    _stepInterval = 0.1; // Fallback to avoid div by zero
    return;
  }
  // Calculate steps per second: speed / degrees_per_step
  float stepsPerSecond = turnSpeed / DEGREES_PER_STEP;
  _stepInterval = 1.0 / stepsPerSecond; // Seconds per step
}

void calculateStepsForAngle(float angle) {
  // Determine direction based on angle sign
  if (angle >= 0) {
    _directionCW = true; // Clockwise
  } else {
    _directionCW = false; // Counter-Clockwise
    angle = -angle; // Work with absolute value for calculation
  }
  
  // Calculate total steps needed
  _stepCount = (int)(angle / DEGREES_PER_STEP);
  
  // Update speed interval immediately
  updateStepInterval();
  
  // Reset state to start moving immediately
  _currState = MOVE;
  _stepsRemaining = _stepCount;
  _stepTime = millis();
  _currentStepIndex = 0; // Start from the beginning of the sequence
  
  // Reset wait timer just in case
  _waitTime = 0;
}

void runStep() {
  // Get the current step pattern from the single array
  const int* step = stepSequence[_currentStepIndex];
  
  // Set the output pins
  setStep(step[0], step[1], step[2], step[3]);
  
  // Update index based on direction
  if (_directionCW) {
    _currentStepIndex++;
    if (_currentStepIndex >= 4) {
      _currentStepIndex = 0; // Wrap around to start
    }
  } else {
    _currentStepIndex--;
    if (_currentStepIndex < 0) {
      _currentStepIndex = 3; // Wrap around to end for CCW
    }
  }
}

// --- Setup ---
void setup() {
  // Initialize pins
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  
  // Initialize Serial
  Serial.begin(9600);
  while (!Serial) { ; } // Wait for serial port (optional for native USB)
  
  // Initial State Setup
  autoMode = AUTO;
  waitInterval = 8.0;
  turnSpeed = 5.0;
  _currState = MOVE;
  
  // Initialize timers to current time to prevent immediate first step
  _stepTime = millis();
  _waitTime = millis();
  
  // Initial calculation (move 0 degrees to set defaults)
  calculateStepsForAngle(0.0); 
  // Ensure we start in MOVE if we want immediate action, or WAIT if idle
  // Based on your logic: "initialized in MOVE state"
  _currState = MOVE;
}

// --- Loop ---
void loop() {
  // --- READ SERIAL ---
  if (Serial.available()) {
    String command = Serial.readString();
    command.trim(); // Remove whitespace
    
    if (command.length() < 2) return; // Need at least 'a' + number
    
    char cmdType = command.charAt(0);
    String payload = command.substring(1);
    
    switch (cmdType) {
      case 'm':
        // 'm0' for AUTO, 'm1' for MANUAL
        if (payload.length() > 0) {
          char modeChar = payload.charAt(0);
          if (modeChar == '0' && autoMode != AUTO) {
            autoMode = AUTO;
            _currState = MOVE;
            _stepsRemaining = _stepCount;
            _stepTime = millis();
          } else if (modeChar == '1' && autoMode != MANUAL) {
            autoMode = MANUAL;
            _currState = MOVE; // Start moving immediately on switch
            _stepsRemaining = _stepCount;
            _stepTime = millis();
          }
        }
        break;
        
      case 'a':
        // 'a90' -> move 90 degrees
        {
          float angle = payload.toFloat();
          calculateStepsForAngle(angle);
        }
        break;
        
      case 's':
        // 's10' -> 10 deg/s
        {
          float speed = payload.toFloat();
          if (speed > 0) {
            turnSpeed = speed;
            updateStepInterval();
            // If currently in MOVE, the new interval applies on next step
          }
        }
        break;
        
      case 'i':
        // 'i5' -> 5 seconds wait
        {
          float interval = payload.toFloat();
          if (interval >= 0) {
            waitInterval = interval;
          }
        }
        break;
    }
  }

  // --- EXECUTION LOGIC ---

  // 1. Check if move sequence is finished
  if (_currState == MOVE && _stepsRemaining == 0) {
    _currState = WAIT;
    _waitTime = millis();
  }

  // 2. Check if we should restart in AUTO mode
  if (_currState == WAIT) {
    if (autoMode == AUTO && (millis() - _waitTime > (unsigned long)(waitInterval * 1000))) {
      _currState = MOVE;
      _stepsRemaining = _stepCount;
      _stepTime = millis(); // Reset step timer to prevent burst
    }
    // If MANUAL: Stay in WAIT indefinitely until new command
  }

  // 3. Execute Step if in MOVE mode
  if (_currState == MOVE) {
    unsigned long currentTime = millis();
    if (currentTime - _stepTime > (unsigned long)(_stepInterval * 1000)) {
      runStep();
      _stepTime = currentTime;
      _stepsRemaining--;
    }
  }
}