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

//TODO:
// x sync bij opstart
// - resync tijdens normale werking?
// x figurendraaien in 2 richtingen:
//  x random 1-5 figuren in 1 richting, dan random 1-5 figuren in andere richting
//  x sync met figuurschijf
// - print's uitschakele voor release

// Info:
// stepper motors: 200 full steps per rotation
// using 1/8e microstepping for smooth stepping, so it is 1600 steps per rotation
// Color disk: is 12.5 steps per color, using 1/8e microstepping, makes 100 microsteps per color
// Figure disk: is 20 steps per figure, using 1/8e microstepping, makes 160 microsteps per color

#define MOVE_DELAY 10000            // delay to next position, millisec
//#define MOVE_MINDELAY 10000           // delay to next position, random minimum, ms
//#define MOVE_MAXDELAY 60000           // delay to next position, random maximum, ms
#define SYNCSPEED 50                  // sync, find HALL sensors, steps/s
#define COLOR_STEPSTONEXT 100         // steps per color
#define COLOR_SYNCOFFSET  0           // steps from HALL sensor pulse to color disk perfect position
#define COLOR_SPEED 100               // steps/s
#define COLOR_ACCEL 200               // acceleration/deceleration in steps/s/s
#define FIGURE_STEPSTONEXT 160        // steps per figure
#define FIGURE_SYNCOFFSET  0          // steps from HALL sensor pulse to figure disk perfect position
#define FIGURE_SPEED 160              // steps/s
#define FIGURE_ACCEL 320              // acceleration/deceleration in steps/s/s

// figure rotation
#define FIGROT_SPEED 50              // figure rotation speed, steps/s
#define FIGROT_ACCEL 200              // acceleration/deceleration in steps/s/s

// Hall sensor threshold: active low, < 1/4th of 5V
#define HALL_LOW_THRES    (1023 / 4)
#define HALL_HIGH_THRES   (1023 / 2)

//  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 state;
unsigned int stepDelay;   // ms
unsigned int millis16;
unsigned int stepTimer;

byte figrotState;
int figrotDir;
byte trigRotationChange;
int8_t rotCounter;

unsigned long printTimer;
char printBuffer[100];

int hall1Value;
int hall1PrevValue;
byte hall1State = 1;  // prevent detection at start
byte hall1Pulse;
byte colDiskSynced;
int colDiskSyncPos;
int hall2Value;
int hall2PrevValue;
byte hall2State = 1;   // prevent detection at start
byte hall2Pulse;
byte figDiskSynced;
int figDiskSyncPos;

void setup() 
{
  Serial.begin(115200);
  Serial.println("Start");

  Serial.println("Start sync");

  // prep for sync
  // use stepping mode for sync to use acceleration/deceleration
  // speed mode has no acceleration/deceleration
  state = 0;
	
  // prep for sync color disk driver
  stepperCol.setCurrentPosition(0);
  stepperCol.setMaxSpeed(SYNCSPEED);
  stepperCol.setAcceleration(COLOR_ACCEL);
  
  // prep for sync figure disk driver
  stepperFig.setCurrentPosition(0);
  stepperFig.setMaxSpeed(SYNCSPEED);
  stepperFig.setAcceleration(FIGURE_ACCEL);

  // prep stepping for figurerotation left-right 
  stepperFigrot.setMaxSpeed(FIGROT_SPEED);
  stepperFigrot.setAcceleration(FIGROT_ACCEL);

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

void loop() 
{
  // hall sensors detect
  // detect low voltage for active

  // syn for color disk
  hall1Value = analogRead(A1);
  hall1Pulse = 0;
  if ((hall1State == 0 ) && (hall1Value < HALL_LOW_THRES) && (hall1PrevValue < HALL_LOW_THRES)) 
  {
    hall1Pulse = 1;
    hall1State = 1;
  } 
  if ((hall1State == 1) && (hall1Value >= HALL_HIGH_THRES) && (hall1PrevValue >= HALL_HIGH_THRES))
  {
      hall1State = 0;
  }
  hall1PrevValue = hall1Value;

  //sprintf(printBuffer, "%d  %d  %d", hall1Value, hall1State, hall1Pulse);
  //Serial.println(printBuffer);

  // sync for figure disk
  hall2Value = analogRead(A0);
  hall2Pulse = 0;
  if ((hall2State == 0) && (hall2Value < HALL_LOW_THRES) && (hall2PrevValue < HALL_LOW_THRES))
  {
    hall2Pulse = 1;
    hall2State = 1;
  }
  if ((hall2State == 1) && (hall2Value >= HALL_HIGH_THRES) && (hall2PrevValue >= HALL_HIGH_THRES))
  {
      hall2State = 0;
  }
  hall2PrevValue = hall2Value;

    
  // motor driving

  millis16 = millis();

  switch(state)
  {
    case 0: // start sync
        stepperCol.move(200*8*3);
        stepperFig.move(200*8*3);
        state = 1;
        stepperCol.run();
        stepperFig.run();
        break;

    case 1: // sync, wait for HALL sensor sync pulse
      if (hall1Pulse && (colDiskSynced == 0))
      {
          colDiskSyncPos = stepperCol.currentPosition();
          stepperCol.move(COLOR_STEPSTONEXT + COLOR_SYNCOFFSET);
          sprintf(printBuffer, "Col disk syncpos: %u", colDiskSyncPos);
          Serial.println(printBuffer);
          colDiskSynced = 1;
      }
      if (hall2Pulse && (figDiskSynced == 0))
      {
          figDiskSyncPos = stepperFig.currentPosition();
          stepperFig.move(FIGURE_STEPSTONEXT + FIGURE_SYNCOFFSET);
          sprintf(printBuffer, "Fig disk syncpos: %u", figDiskSyncPos);
          Serial.println(printBuffer);
          figDiskSynced = 1;
      }
      // both synced?
      if (colDiskSynced & figDiskSynced)
      {
          Serial.println("Synced1");
          stepTimer = millis16;
          state = 2;
      }
      stepperCol.run();
      stepperFig.run();
      break;

    case 2:
      // wait till motors stopped
      if ((stepperCol.distanceToGo() == 0) && (stepperFig.distanceToGo() == 0))
      {
          sprintf(printBuffer, "Synced2: col_pos = %ld, fig_pos = %ld", stepperCol.currentPosition(), stepperFig.currentPosition());
          Serial.println(printBuffer);
          stepperCol.setMaxSpeed(COLOR_SPEED);
          stepperFig.setMaxSpeed(FIGURE_SPEED);
          stepTimer = millis16;
          state = 10;
      }
      stepperCol.run();
      stepperFig.run();
      break;
      
    case 10: // delay before move to next step
      if ((millis16 - stepTimer) >= MOVE_DELAY)
      {
        stepperCol.move(COLOR_STEPSTONEXT);
        stepperFig.move(FIGURE_STEPSTONEXT);
		    trigRotationChange = 1;					   
		    stepTimer = millis16;
      }
      stepperCol.run();
      stepperFig.run();
      break;
    
  }

  // figure rotation driving
  switch(figrotState)
  {
    case 0: //start stepping
      stepperFigrot.setCurrentPosition(0);
      figrotDir = 10000;
      stepperFigrot.move(figrotDir);
      rotCounter = 3;
      sprintf(printBuffer, "New rotCounter=%d", rotCounter);
      figrotState = 1;
      break;
      
    case 1: // wait
      if (trigRotationChange)
      {
        trigRotationChange = 0;
        --rotCounter;
        sprintf(printBuffer, "rotCounter=%d", rotCounter);
        if (rotCounter <= 0)
        {
          figrotDir = -figrotDir; 
          rotCounter = (byte)random(1,6);   // 1-5 (excl max)
          sprintf(printBuffer, "New rotCounter=%d", rotCounter);
          Serial.println(printBuffer);
        }
        stepperFigrot.move(figrotDir);
        //figrotState = 1;
      }
      break;
  }
  stepperFigrot.run();
 
/*
  if (millis() - printTimer >= 100)
  {
    sprintf(printBuffer, "%d  %d  ", colDiskSynced, figDiskSynced);
    Serial.println(printBuffer);
    printTimer = millis();
  }
  */

}