// --- 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;

// Speed Limits (Clamping)
const float MAX_TURN_SPEED = 45.0; // Maximum safe speed in deg/s
const float MIN_TURN_SPEED = 0.1;  // Minimum safe speed to avoid div by zero

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

Mode autoMode = AUTO;          // Default: AUTO
float targetAngle = 15.0;      // Target angle in degrees
float turnSpeed = 45.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() {
  // 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
    }
  }
}

// --- Serial Output Helpers ---

void printStatus() {
  Serial.print("Mode: ");
  Serial.print(autoMode == AUTO ? "AUTO" : "MANUAL");
  Serial.print(" | State: ");
  Serial.print(_currState == MOVE ? "MOVE" : "WAIT");
  Serial.print(" | Steps Left: ");
  Serial.print(_stepsRemaining);
  Serial.print(" | Speed: ");
  Serial.print(turnSpeed);
  Serial.print(" deg/s | Wait: ");
  Serial.print(waitInterval);
  Serial.println("s");
}

void printCommandAck(char cmd, String payload) {
  Serial.print("> Command '");
  Serial.print(cmd);
  Serial.print(payload);
  Serial.println("' received.");
}

void printError(String msg) {
  Serial.print("ERROR: ");
  Serial.println(msg);
}

// --- 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)
  
  Serial.println("=== Stepper Motor Controller Initialized ===");
  
  // 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(targetAngle); 
  _currState = MOVE;
  
  Serial.println("Ready. Commands: m[0|1], a[deg], s[deg/s], i[sec]");
  Serial.println("  (Speed clamped between ");
  Serial.print(MIN_TURN_SPEED);
  Serial.print(" and ");
  Serial.print(MAX_TURN_SPEED);
  Serial.println(" deg/s)");
  printStatus();
}

// --- Loop ---
void loop() {
  // --- READ SERIAL ---
  if (Serial.available()) {
    String command = Serial.readString();
    command.trim(); // Remove whitespace
    
    if (command.length() < 2) {
      printError("Invalid command format (too short).");
      return;
    }
    
    char cmdType = command.charAt(0);
    String payload = command.substring(1);
    
    printCommandAck(cmdType, payload);
    
    switch (cmdType) {
      case 'm':
        // 'm0' for AUTO, 'm1' for MANUAL
        if (payload.length() > 0) {
          char modeChar = payload.charAt(0);
          if (modeChar == '0') {
            if (autoMode != AUTO) {
              autoMode = AUTO;
              _currState = MOVE;
              _stepsRemaining = _stepCount;
              _stepTime = millis();
              Serial.println("  -> Mode switched to AUTO. Starting move cycle.");
            } else {
              Serial.println("  -> Already in AUTO mode.");
            }
          } else if (modeChar == '1') {
            if (autoMode != MANUAL) {
              autoMode = MANUAL;
              _currState = MOVE; // Start moving immediately on switch
              _stepsRemaining = _stepCount;
              _stepTime = millis();
              Serial.println("  -> Mode switched to MANUAL. Moving once, then waiting.");
            } else {
              Serial.println("  -> Already in MANUAL mode.");
            }
          } else {
            printError("Invalid mode. Use 'm0' (AUTO) or 'm1' (MANUAL).");
          }
        } else {
          printError("Missing mode argument (0 or 1).");
        }
        break;
        
      case 'a':
        // 'a90' -> move 90 degrees
        {
          float angle = payload.toFloat();
          if (payload.length() == 0 || (angle == 0 && payload != "0")) {
            printError("Invalid angle format.");
          } else {
            calculateStepsForAngle(angle);
            targetAngle = angle;
            Serial.print("  -> Moving ");
            Serial.print(_stepCount);
            Serial.print(" steps (");
            Serial.print(targetAngle);
            Serial.println(" degrees).");
            if (_stepCount == 0) {
              Serial.println("  -> No movement required (0 degrees).");
            }
          }
        }
        break;
        
      case 's':
        // 's10' -> 10 deg/s
        {
          float speed = payload.toFloat();
          if (payload.length() == 0 || speed <= 0) {
            printError("Invalid speed. Must be > 0.");
          } else {
            turnSpeed = speed;
            
            // Logic to check if clamping occurred
            float actualSpeed = turnSpeed;
            if (actualSpeed > MAX_TURN_SPEED) {
              turnSpeed = MAX_TURN_SPEED;
              Serial.print("  -> WARNING: Speed clamped to max ");
            } else if (actualSpeed < MIN_TURN_SPEED) {
              turnSpeed = MIN_TURN_SPEED;
              Serial.print("  -> WARNING: Speed clamped to min ");
            }
            
            updateStepInterval();
            Serial.print("  -> Speed set to ");
            Serial.print(turnSpeed);
            Serial.print(" deg/s (Step interval: ");
            Serial.print(_stepInterval * 1000, 2);
            Serial.println(" ms).");
          }
        }
        break;
        
      case 'i':
        // 'i5' -> 5 seconds wait
        {
          float interval = payload.toFloat();
          if (payload.length() == 0 || interval < 0) {
            printError("Invalid interval. Must be >= 0.");
          } else {
            waitInterval = interval;
            Serial.print("  -> Wait interval set to ");
            Serial.print(waitInterval);
            Serial.println(" seconds.");
          }
        }
        break;
        
      default:
        printError("Unknown command. Use m, a, s, or i.");
        break;
    }
    
    // Print status after every command for immediate feedback
    printStatus();
  }

  // --- EXECUTION LOGIC ---

  // 1. Check if move sequence is finished
  if (_currState == MOVE && _stepsRemaining == 0) {
    _currState = WAIT;
    _waitTime = millis();
    Serial.println("--- State: MOVE -> WAIT ---");
    if (autoMode == AUTO) {
      Serial.print("  Waiting ");
      Serial.print(waitInterval);
      Serial.println(" seconds before next cycle...");
    } else {
      Serial.println("  Waiting indefinitely for new command (MANUAL mode).");
    }
  }

  // 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
      Serial.println("--- State: WAIT -> MOVE ---");
      Serial.print("  Auto-restart! Moving ");
      Serial.print(_stepsRemaining);
      Serial.println(" steps.");
    }
    // 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--;
      
      // Optional: Print progress every 10 steps to avoid spam
      // if (_stepCount > 0 && (_stepCount - _stepsRemaining) % 10 == 0) {
      //   Serial.print("  Step: ");
      //   Serial.print(_stepCount - _stepsRemaining);
      //   Serial.print("/");
      //   Serial.println(_stepCount);
      // }
    }
  }
}