/*  AccelStepper6AxisRampsMega.ino

This simulation demonstrates bouncing 6 stepper motors between different
limits while simultaneously reading potentiomters to adust speed 
acceleration, and target positions during moves.

It also demonstrates use of an array of classes in order to manage them
together as a set.

https://wokwi.com/projects/411859783629453313

  // built from :
  // https://wokwi.com/projects/390741120778917889

*/
// For RAMPS 1.4, Copied from https://wokwi.com/projects/390741120778917889
#define X_DIR_PIN          55
#define X_STEP_PIN         54
#define X_ENABLE_PIN       38

#define Y_DIR_PIN          61
#define Y_STEP_PIN         60
#define Y_ENABLE_PIN       56

#define Z_DIR_PIN          48
#define Z_STEP_PIN         46
#define Z_ENABLE_PIN       62

#define A_DIR_PIN          28
#define A_STEP_PIN         26
#define A_ENABLE_PIN       24

#define B_DIR_PIN          34
#define B_STEP_PIN         36
#define B_ENABLE_PIN       30
// 
#define C_DIR_PIN          32
#define C_STEP_PIN         47
#define C_ENABLE_PIN       45

#import <AccelStepper.h> // https://www.airspayce.com/mikem/arduino/AccelStepper/
// and https://github.com/waspinator/AccelStepper

// An array of classes, 
AccelStepper steppers[] = {
  {AccelStepper::DRIVER, X_STEP_PIN, X_DIR_PIN},
  {AccelStepper::DRIVER, Y_STEP_PIN, Y_DIR_PIN},
  {AccelStepper::DRIVER, Z_STEP_PIN, Z_DIR_PIN},
  {AccelStepper::DRIVER, A_STEP_PIN, A_DIR_PIN},
  {AccelStepper::DRIVER, B_STEP_PIN, B_DIR_PIN},
  {AccelStepper::DRIVER, C_STEP_PIN, C_DIR_PIN},
};
// parallel array for enable pins
const int StepperEnablePins[] = {X_ENABLE_PIN, Y_ENABLE_PIN, Z_ENABLE_PIN, A_ENABLE_PIN, B_ENABLE_PIN, C_ENABLE_PIN };
// parallel array for limit buttons
const int ButtonPins[] = {3, 14, 18, 27, 29, 25};

const int NUM_STEPPERS = sizeof(steppers) / sizeof(steppers[0]);

void usage() {
  Serial.println(__FILE__ " on " __DATE__);
  Serial.println(F(R"(AccelStepper6AxisRAMPSMega.ino https://wokwi.com/projects/411859783629453313
Speed, acceleration change with potentiometers.
Bounce position changes with button press. )"));
}
const int PositionPin = A13, SpeedPin = A14, AccelPin = A15;
uint16_t position, speed, acceleration, lastPosition=-1, lastSpeed=-1, lastAccel=-1; // pot-read

void readAllInputs() {
  uint32_t now = millis();
  const auto interval = 200;
  static auto last = - interval;
  if (now - last >= interval) {
    now += interval;
    speed = map(analogRead(SpeedPin), 0, 1023, 0, 2000);
    acceleration = map(analogRead(AccelPin), 0, 1023, 0, 2000);
    position = map(analogRead(PositionPin), 0, 1023, 0, 2000);
    int ii = 0;
    for (auto pin : ButtonPins) {
      if (digitalRead(pin) == LOW && steppers[ii].targetPosition() != position) {
        steppers[ii].moveTo(position);
        Serial.print("Retargeting stepper ");
        Serial.print(ii);
        Serial.print(" to ");
        Serial.println(position);
      }
      ++ii;
    }
  }
}

void process() {
  if (acceleration != lastAccel || speed != lastSpeed) {
    for ( auto & stepper : steppers) {
      stepper.setAcceleration(acceleration);
      stepper.setMaxSpeed(speed);
      lastSpeed = speed;
      lastAccel = acceleration;
    }
    Serial.print("Speed:");
    Serial.print(speed);
    Serial.print(" Acceleration:");
    Serial.println(acceleration);
  }
}

void enableAllSteppers() {
  int ii = 0;
  for (auto &stepper : steppers) {
    stepper.setEnablePin(StepperEnablePins[ii]);
    stepper.setPinsInverted(false, false, true);
    stepper.enableOutputs();
    stepper.setMaxSpeed(1000);
    stepper.setAcceleration(100);
    stepper.moveTo(random(2000));
    pinMode(ButtonPins[ii], INPUT_PULLUP);
    ++ii;
  }
}

void runAllSteppers() {
  int ii = 0 ;
  for (auto &stepper : steppers) {
    if (stepper.distanceToGo() == 0) {
      if(stepper.currentPosition() == 0 ){
        stepper.moveTo(position);      
      } else { // bounce
        stepper.moveTo(-stepper.currentPosition());
      }
      Serial.print(ii);
      Serial.print("@");
      Serial.print(stepper.currentPosition());
      Serial.print(" ");
    }
    ++ii;
    stepper.run();
  }
}

// Arduino required #####################################

void setup() {
  srandom( 20241015 + analogRead(SpeedPin) + analogRead(AccelPin));
  Serial.begin(115200);
  enableAllSteppers();
  usage();
}

void loop() {
  readAllInputs();
  process();
  runAllSteppers();
}