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

// Assemble an array of strutures
struct MyStruct {
  const char *name;
  const AccelStepper &stepper;
  int enablePin;
  int button;
} stepperAssemblies[] = {
  {"X", steppers[0], X_ENABLE_PIN,3},
  {"Y", steppers[1], Y_ENABLE_PIN,14},
  {"Z", steppers[2], Z_ENABLE_PIN,18},
  {"A", steppers[3], A_ENABLE_PIN,27},
  {"B", steppers[4], B_ENABLE_PIN,29},
  {"C", steppers[5], C_ENABLE_PIN,25},
};

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

void usage() {
  Serial.print(__FILE__ " on " __DATE__);
  Serial.print(F(R"(AccelStepper6AxisRAMPSMega.ino )"));
}
const int PositionPin = A13, SpeedPin = A14, AccelPin = A15;
uint16_t position, speed, acceleration, lastPosition, lastSpeed, lastAccel; // 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);
    // // Parallel array form
    // int ii = 0;
    // for (auto pin : ButtonPins) {
    //   if (digitalRead(pin) == LOW) {
    //     steppers[ii].moveTo(position);
    //   }
    //   ++ii;
    // }
    // array of struct form:
    for (auto &obj : stepperAssemblies) {
      if (digitalRead(obj.button) == LOW) {
        obj.stepper.moveTo(position);
      }
    }
  }
}

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 enableAllSteppersParallel() {
  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 enableAllSteppers() {
  int ii = 0;
  for (auto &obj : stepperAssemblies) {
    obj.stepper.setEnablePin(obj.enablePin);
    obj.stepper.setPinsInverted(false, false, true);
    obj.stepper.enableOutputs();
    obj.stepper.setMaxSpeed(1000);
    obj.stepper.setAcceleration(100);
    obj.stepper.moveTo(random(2000));
    pinMode(obj.button, INPUT_PULLUP);
    Serial.print("Initialized ");
    Serial.print(obj.name);
    Serial.print(" Stepper, enabe Button Pin:");
    Serial.println(obj.button);
    ++ii;
  }
}


void runAllSteppers() {
  int ii = 0 ;
  for (auto &stepper : steppers) {
    if (stepper.distanceToGo() == 0) {
      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();
  enableAllSteppers();
  usage();
}

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