// Include the AccelStepper library
#include <AccelStepper.h>

//TODO: sync bij opstart
//TODO: resync tijdens normale werking? 
//TODO: figurendraaien in 2 richtingen:
//  - random 1-5 figuren in 1 richting, dan random 1-5 figuren in andere richting
//  - sync met figuurschijf

// Color disk: is 12.5 steps per color, using 1/8e microstepping, so 100 microsteps per color
#define COLOR_DELAY 5000              // delay for color change, millisec
#define COLOR_STEPSTONEXT 100         // steps per color
#define COLOR_SPEED 100               // steps/s
#define COLOR_ACCEL 200               // acceleration/deceleration in steps/s/s
#define COLOR_SYNCSPEED 10            // sync steps/s
#define COLOR_MAXSYNCSTEPS 1000000        // max speps to find sync
#define COLOR_FAILSPEED 1             // fallback speed after sync failed

// Figure disk: is 20 steps per figure, using 1/8e microstepping, so 160 microsteps per color
#define FIGURE_DELAY 10000             // delay for figure change, millisec
#define FIGURE_STEPSTONEXT 160        // steps per figure
#define FIGURE_SPEED 100              // steps/s
#define FIGURE_ACCEL 200              // acceleration/deceleration in steps/s/s
#define FIGURE_SYNCSPEED 10           // sync steps/s
#define FIGURE_MAXSYNCSTEPS 1000000       // max speps to find sync
#define FIGURE_FAILSPEED 1            // fallback speed after sync failed

#define FIGUREROTATE_SPEED 50          // figure rotation speed, steps/s

//  AccelStepper objects, mode 1=driver, step pin, dir pin  // 2,5; 3,6; 4,7  
AccelStepper stepperCol = AccelStepper(1, 2, 5);  
AccelStepper stepperFig = AccelStepper(1, 4, 7);
AccelStepper stepperFigRot = AccelStepper(1, 3, 6);

byte colorState;
unsigned long colorTimer;
byte figureState;
unsigned long figureTimer;
byte inpBytePrev;
unsigned long printTimer;

void setup() 
{
  Serial.begin(115200);

  // color driver init
  stepperCol.setMaxSpeed(COLOR_SYNCSPEED);
  stepperCol.setAcceleration(COLOR_ACCEL);
  
  // Figure driver init
  stepperFig.setMaxSpeed(FIGURE_SYNCSPEED);
  stepperFig.setAcceleration(FIGURE_ACCEL);

  // figure rotation driver init
  stepperFigRot.setMaxSpeed(FIGUREROTATE_SPEED);
  stepperFigRot.setSpeed(FIGUREROTATE_SPEED);
  // acceleration werkt niet bij runSpeed
  
  colorState = 1;
  figureState = 1;

  // activate all motor drivers, active low
  pinMode(8, OUTPUT);
  digitalWrite(8, 0);  
    
}

void loop() 
{
 
  // color disk driving
  switch(colorState)
  {
    case 0: // simple run
        stepperCol.setSpeed(2);
        stepperCol.runSpeed();
        break;

    case 1: //start stepping
      stepperCol.setCurrentPosition(0);
      stepperCol.setMaxSpeed(COLOR_SPEED);
      stepperCol.move(COLOR_STEPSTONEXT);
      colorTimer = millis();
      colorState = 2;
      stepperCol.run();
      break;
      
    case 2: // delay
      if (millis() - colorTimer >= 5000)
      {
        stepperCol.move(COLOR_STEPSTONEXT);
        colorTimer = millis();
        //colorState = 2;
      }
      stepperCol.run();
      break;
    
  }

  // figure disk driving
  switch(figureState)
  {
    case 0: // simple run
        stepperFig.setSpeed(2);
        stepperFig.runSpeed();
        break;

    case 1: //start stepping
      stepperFig.setCurrentPosition(0);
      stepperFig.setMaxSpeed(FIGURE_SPEED);
      stepperFig.move(FIGURE_STEPSTONEXT);
      figureTimer = millis();
      figureState = 2;
      stepperFig.run();
      break;
      
    case 2: // delay
      if (millis() - figureTimer >= 10000)
      {
        stepperFig.move(FIGURE_STEPSTONEXT);
        figureTimer = millis();
        //figureState = 2;
      }
      stepperFig.run();
      break;
    
  }

  // figure rotation is constant speed
  stepperFigRot.runSpeed();
  
/*
  if (millis() - printTimer >= 1000)
  {
    Serial.print(figureState); 
    Serial.print("  ");
    Serial.println(figureTimer); 
    printTimer = millis();
  }
  */
  //
}