#include <AccelStepper.h>

// Define the stepper motor connections
#define BOOM_STEP_PIN 2
#define BOOM_DIR_PIN 3
#define TURNTABLE_STEP_PIN 4
#define TURNTABLE_DIR_PIN 5
#define WINCH_STEP_PIN 6
#define WINCH_DIR_PIN 7

// Define the pedometer and switch pins
#define PEDOMETER_BOOM A0
#define PEDOMETER_TURNTABLE A1
#define PEDOMETER_WINCH A2
#define SWITCH_PIN 12

// Create stepper motor objects
AccelStepper boomStepper(AccelStepper::DRIVER, BOOM_STEP_PIN, BOOM_DIR_PIN);
AccelStepper turntableStepper(AccelStepper::DRIVER, TURNTABLE_STEP_PIN, TURNTABLE_DIR_PIN);
AccelStepper winchStepper(AccelStepper::DRIVER, WINCH_STEP_PIN, WINCH_DIR_PIN);

bool winchAtZero = true;

void setup() {
  // Set the maximum acceleration for each stepper motor
  boomStepper.setAcceleration(500); // Adjust the acceleration as needed
  turntableStepper.setAcceleration(500);
  winchStepper.setAcceleration(500);

  // Set the initial positions to 0 for each stepper motor
  boomStepper.setCurrentPosition(0);
  turntableStepper.setCurrentPosition(0);
  winchStepper.setCurrentPosition(0);

  // Seed the random number generator
  randomSeed(analogRead(0));

  // Set the switch pin as input with a pull-up resistor
  pinMode(SWITCH_PIN, INPUT_PULLUP);

  // Serial communication for debugging (optional)
  Serial.begin(9600);
}

void loop() {
  // Read the state of the switch
  int switchState = digitalRead(SWITCH_PIN);

  // If the switch is in the ON position, proceed with stepper motor movements
  if (switchState == HIGH) {
    if (winchAtZero) {
      // Move Winch to 0 position
      winchStepper.setMaxSpeed(200);
      winchStepper.moveTo(0);
      while (winchStepper.distanceToGo() != 0) {
        winchStepper.run();
      }
      winchAtZero = false;
    }

    // Read the values from the pedometers
    int boomPedometerValue = analogRead(PEDOMETER_BOOM);
    int turntablePedometerValue = analogRead(PEDOMETER_TURNTABLE);
    int winchPedometerValue = analogRead(PEDOMETER_WINCH);

    // Map pedometer values to speed and range for each stepper motor
    int boomSpeed = map(boomPedometerValue, 0, 1023, 100, 1000); // Adjust the speed range as needed
    int boomRange = map(boomPedometerValue, 0, 1023, 500, 3000); // Adjust the range as needed

    int turntableSpeed = map(turntablePedometerValue, 0, 1023, 50, 500);
    int turntableRange = map(turntablePedometerValue, 0, 1023, 500, 2000); // Adjust the range as needed

    int winchSpeed = map(winchPedometerValue, 0, 1023, 200, 800);
    int winchRange = map(winchPedometerValue, 0, 1023, 0, 500);

    // Move Boom and Turntable to their respective target positions with the calculated speeds
    boomStepper.setMaxSpeed(boomSpeed);
    boomStepper.moveTo(random(boomRange));

    turntableStepper.setMaxSpeed(turntableSpeed);
    turntableStepper.moveTo(random(turntableRange));

    // Wait for Boom and Turntable to reach their target positions
    while (boomStepper.distanceToGo() != 0 || turntableStepper.distanceToGo() != 0) {
      boomStepper.run();
      turntableStepper.run();
    }

    // Move Winch to its random position with the calculated speed
    winchStepper.setMaxSpeed(winchSpeed);
    winchStepper.moveTo(random(winchRange));

    // Wait for Winch to reach its target position
    while (winchStepper.distanceToGo() != 0) {
      winchStepper.run();
    }

    // Reset the flag after the sequence is complete
    winchAtZero = true;

    // Adjust the wait time based on the boom pedometer value
    int waitTime = map(boomPedometerValue, 0, 1023, 500, 2000); // Adjust the wait time range as needed
    delay(waitTime);
  } else {
    // If the switch is in the OFF position, stop all stepper motors
    boomStepper.stop();
    turntableStepper.stop();
    winchStepper.stop();

    // You might also want to set the stepper positions to the current positions
    // to resume from the same point when the switch is turned back ON.
    boomStepper.setCurrentPosition(boomStepper.currentPosition());
    turntableStepper.setCurrentPosition(turntableStepper.currentPosition());
    winchStepper.setCurrentPosition(winchStepper.currentPosition());
  }
}