// Work in progress
// This moves a set of steppers towards an random vector of locations.
// https://wokwi.com/projects/390741120778917889
// for https://forum.arduino.cc/t/controlling-many-stepper-motors-beginner/1228155/16
//
// v1: V1_simultaneousSteps.ino.txt tab -- move all steppers at same rate
// v2: Move steppers at independent rates to reach new target at the same time.
// v3: Use Bresenham algorithm to sync axes, so you could accelerate and keep in step.
//
// Pasted together from bits of these:
// https://wokwi.com/projects/356666059490147329 -- non-blocking FastLED
// https://wokwi.com/projects/390478474763185153 -- 6axis RAMPS GCODE interpreter
// https://forum.arduino.cc/t/changing-button-states-from-momentary-to-latch/993706/14 -- handling vectors of parts with structs
//
// Serial plotter shows decreasing error as steppers approach new targets

#include <FastLED.h>

#define NUM_LEDS  6
const int FASTLED_PIN  = 2;
const int SpeedPin = A15;
const int PatternPin = A14;
const int verbose = 4;

// FastLED variables
CRGB leds[NUM_LEDS];
uint8_t patternCounter = 0;
bool isRunning = false;
bool changePattern = false;

void checkTimer();
int reportInterval = 100; // ms

// FIREglobals:
const uint8_t cooling = 55;
const uint8_t sparking = 120;
bool gReverseDirection = false;

//  timing coefficients for fast dummy axis
uint32_t RetargetInterval = -1; //ms
const int msPerStep = 3;  // if you want acceleration, vary this step rate and the others will follow 
// Bresenham fast dummy axis delta
int32_t dx = RetargetInterval / msPerStep; // ghost axis dx for Bresenham Algorithm synchronization

struct Steppers {
  int8_t StepPin;
  int8_t DirPin;
  int8_t EnablePin;
  int8_t MinPin;
  int8_t MaxPin;
  long position;
  long target;
  //  uint32_t lastStep; //
  //  uint32_t interval; // ms between steps
  int32_t dy; // bresenham dy  (ghost axis dx=constant is faster than everything)
  int32_t D; // bresenham Decision sum

} steppers[] = { // For RAMPS board with extra stepper
  {54,55,38,3}, // RAMPS X per  RAMPS pins from https://reprap.org/wiki/RAMPS_1.4#Pins & https://forum.arduino.cc/t/arduino-mega-use-analog-pins-as-digital/13844/6?u=davex
  {60,61,56,14}, // RAMPS Y
  {46,48,62,18}, // RAMPS Z
  {26,28,24,27}, // RAMP E0
  {36,34,30,29}, // RAMPS E1
  {16,17,23,25}, // Stepper on other RAMPS end pins
};


void updateRetargetInterval(uint32_t newInterval){
  if(newInterval != RetargetInterval){
    RetargetInterval = newInterval;
    dx =  RetargetInterval / msPerStep; // set speed to reach the target by the end of the interval
  }
}


void setup(void) {
  Serial.begin(115200);  // open coms

  FastLED.addLeds<WS2812B, FASTLED_PIN, GRB>(leds, NUM_LEDS);
  FastLED.setBrightness(100);

  for (auto &stepper : steppers) {
    pinMode(stepper.DirPin, OUTPUT);
    pinMode(stepper.StepPin, OUTPUT);
    pinMode(stepper.EnablePin, OUTPUT);
    pinMode(stepper.MinPin, INPUT_PULLUP);
    digitalWrite(stepper.EnablePin, LOW);
  }
  updateRetargetInterval(2000);
}

void loop(void) {
  uint32_t now = millis();
  static uint32_t targetInterval = RetargetInterval;
  static uint32_t lastLedChange = -targetInterval;
  if (now - lastLedChange >= targetInterval) {
    lastLedChange = now;
    switch(map(analogRead(PatternPin),0,1024,0,2)){
      case 0:
        rainbowLoop(1,17);
        break;
      case 1:
        fireLoop();
        break;
    }

    FastLED.show();
    updateRetargetInterval(map(analogRead(SpeedPin),0,1023,2000,500));
    targetInterval = RetargetInterval;
  }

  stepperSyncWithLeds(now);
  report();
}

void stepperSyncWithLeds(uint32_t now) {
  static uint32_t last = 0;
  if (now - last < msPerStep) return;
  last = now;
  int i = 0;
  for (auto &stepper : steppers) {
    if (stepper.target != leds[i][0]) { // new target, set slope
      stepper.target = leds[i][0]; // red channel
      long distance_to_go = stepper.target - stepper.position;
      stepper.dy = abs(distance_to_go);
      stepper.D = 2 * stepper.dy - dx;

    }

    if (stepper.position != stepper.target && stepper.D > 0 ) { // time to step
      stepper.D -= 2 * dx; // decrement the Decision rule by the slope effect of one dy
      //      stepper.lastStep = now;
      if (stepper.position < stepper.target) {
        digitalWrite(stepper.DirPin, HIGH);
        ++stepper.position;
      } else {
        digitalWrite(stepper.DirPin, LOW);
        --stepper.position;
      }
      digitalWrite(stepper.StepPin, HIGH);
      digitalWrite(stepper.StepPin, LOW);
    }
    stepper.D += 2 * stepper.dy; // increment y Decision rule by the slope effect of one of dx
    ++i;
  }
}


void fireLoop() {
  static byte heat[NUM_LEDS];

  // Step 1.  Cool down every cell a little
  for ( int i = 0; i < NUM_LEDS; i++) {
    heat[i] = qsub8( heat[i],  random8(0, ((cooling * 10) / NUM_LEDS) + 2));
  }

  // Step 2.  Heat from each cell drifts 'up' and diffuses a little
  for ( int k = NUM_LEDS - 1; k >= 2; k--) {
    heat[k] = (heat[k - 1] + heat[k - 2] + heat[k - 2] ) / 3;
  }

  // Step 3.  Randomly ignite new 'sparks' of heat near the bottom
  if ( random8() < sparking ) {
    int y = random8(3);
    heat[y] = qadd8( heat[y], random8(160, 255) );
  }

  // Step 4.  Map from heat cells to LED colors
  for ( int j = 0; j < NUM_LEDS; j++) {
    CRGB color = HeatColor( heat[j]);
    int pixelnumber;
    if ( gReverseDirection ) {
      pixelnumber = (NUM_LEDS - 1) - j;
    } else {
      pixelnumber = j;
    }
    leds[pixelnumber] = color;
  }
}

  void rainbowLoop(uint8_t thisSpeed, uint8_t deltaHue) {     // The fill_rainbow call doesn't support brightness levels.
 
 uint8_t thisHue = beatsin8(thisSpeed,0,255);                // A simple rainbow wave.
 //uint8_t thisHue = beat8(thisSpeed,255);                     // A simple rainbow march.
  
 fill_rainbow(leds, NUM_LEDS, thisHue, deltaHue);            // Use FastLED's fill_rainbow routine.
 
}


void report(void) {
  const uint32_t interval = 100;
  static uint32_t last = -interval;
  if (millis() - last < interval) return;
  last += interval;
  for ( auto &stepper : steppers) {
    Serial.print(stepper.target - stepper.position);
    Serial.print(", ");
  }
  Serial.println();
}