#include <Arduino.h>

const int MOTOR_COUNT = 3;  // We're using 3 motors (X, Y, Z)

// Pin definitions for motor control
const int EN[MOTOR_COUNT] = {13, 10, 7};    // Enable pins
const int STEP[MOTOR_COUNT] = {12, 9, 6};   // Step pins
const int DIR[MOTOR_COUNT] = {11, 8, 5};    // Direction pins

// Pin definitions for direction switches
#define NEG_X 21  // -X
#define POS_X 20  // +X
#define NEG_Y 19  // -Y
#define POS_Y 18  // +Y
#define NEG_Z 17  // -Z
#define POS_Z 16  // +Z

// Pin definitions for mode selection switches
#define MODE_1MM 14  // 1mm mode (10 degrees)
#define MODE_10MM 15 // 10mm mode (100 degrees)

// Motor movement parameters
const int stepDelay = 800;  // Microseconds between steps (controls speed)
bool isModeHighResolution = true; // Default to 1mm (10 degree) mode

// Steps per rotation, assuming 200 steps per full rotation (1.8 degrees per step)
const int STEPS_PER_ROTATION = 200;

// Steps required for different angular movements
const int STEPS_FOR_10_DEGREES = 10; // 360*10/1.8 = 2,000
const int STEPS_FOR_100_DEGREES = 100; // 360*100/1.8 = 20,000

void setup() {
  // Initialize motor control pins
  for (int i = 0; i < MOTOR_COUNT; i++) {
    pinMode(EN[i], OUTPUT);
    pinMode(STEP[i], OUTPUT);
    pinMode(DIR[i], OUTPUT);
    
    digitalWrite(EN[i], HIGH);  // Disable motors initially (HIGH = disabled for most drivers)
  }
  
  // Initialize direction buttons with pull-up resistors
  pinMode(NEG_X, INPUT_PULLUP);
  pinMode(POS_X, INPUT_PULLUP);
  pinMode(NEG_Y, INPUT_PULLUP);
  pinMode(POS_Y, INPUT_PULLUP);
  pinMode(NEG_Z, INPUT_PULLUP);
  pinMode(POS_Z, INPUT_PULLUP);
  
  // Initialize mode selection switches with pull-up resistors
  pinMode(MODE_1MM, INPUT_PULLUP);
  pinMode(MODE_10MM, INPUT_PULLUP);
  
  Serial.begin(9600);
  Serial.println("Stepper Motor Control System Ready");
  Serial.println("Default mode: 1mm (10 degrees)");
}


// Function to move a specific motor a specific number of steps
void moveMotorSteps(int motorIndex, bool clockwise, int steps) {
    // Set direction
    digitalWrite(DIR[motorIndex], clockwise ? LOW : HIGH);
    
    // Enable motor
    digitalWrite(EN[motorIndex], LOW);
    
    // Step the motor the specified number of steps
    for(int i = 0; i < steps; i++) {
      digitalWrite(STEP[motorIndex], HIGH);
      delayMicroseconds(stepDelay);
      digitalWrite(STEP[motorIndex], LOW);
      delayMicroseconds(stepDelay);
    }
  }
  
  // Function to stop a specific motor
  void stopMotor(int motorIndex) {
    digitalWrite(EN[motorIndex], HIGH);  // Disable motor
  }

void loop() {
  // Check mode switches - prioritize 1mm if both are pressed
  if (digitalRead(MODE_1MM) == LOW) {
    isModeHighResolution = true;
    Serial.println("Mode: 1mm (10 degrees per click)");
    delay(300); // Debounce
  } 
  else if (digitalRead(MODE_10MM) == LOW) {
    isModeHighResolution = false;
    Serial.println("Mode: 10mm (100 degrees per click)");
    delay(300); // Debounce
  }
  
  // Check X-axis switches
  if (digitalRead(NEG_X) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(0, false, steps);  // Move X motor counter-clockwise
    delay(100); // Small delay to avoid rapid repeated movements
  } 
  else if (digitalRead(POS_X) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(0, true, steps);   // Move X motor clockwise
    delay(100); // Small delay to avoid rapid repeated movements
  }
  else {
    stopMotor(0);         // Stop X motor if no switch is pressed
  }
  
  // Check Y-axis switches
  if (digitalRead(NEG_Y) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(1, false, steps);  // Move Y motor counter-clockwise
    delay(100); // Small delay to avoid rapid repeated movements
  } 
  else if (digitalRead(POS_Y) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(1, true, steps);   // Move Y motor clockwise
    delay(100); // Small delay to avoid rapid repeated movements
  }
  else {
    stopMotor(1);         // Stop Y motor if no switch is pressed
  }
  
  // Check Z-axis switches
  if (digitalRead(NEG_Z) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(2, false, steps);  // Move Z motor counter-clockwise
    delay(50); // Small delay to avoid rapid repeated movements
  } 
  else if (digitalRead(POS_Z) == LOW) {
    int steps = isModeHighResolution ? STEPS_FOR_10_DEGREES : STEPS_FOR_100_DEGREES;
    moveMotorSteps(2, true, steps);   // Move Z motor clockwise
    delay(100); // Small delay to avoid rapid repeated movements
  }
  else {
    stopMotor(2);         // Stop Z motor if no switch is pressed
  }
}