#define dirPin 6 //2
#define stepPin 3
#define button_ForwardReverse 5
#define button_HaltStart 4
#define button_Microstep 2 //6
#define MS1 9
#define MS2 8
#define MS3 7

boolean M01,M02,M11,M12,M21,M22,
HaltStart,ForwardReverse,TrigMicrostep,lastTrigMicrostep,
P1,P2,P3 = LOW;

int Microstep = 0;
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;

boolean MicrostepArray[5][3] = {
 {LOW, LOW, LOW},
 {HIGH, LOW, LOW},
 {LOW, HIGH, LOW},
 {HIGH, HIGH, LOW},
 {HIGH, HIGH, HIGH} 
};

void setup() {
  Serial.begin(115200); // Any baud rate should work
 
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(MS1, OUTPUT);
  pinMode(MS2, OUTPUT);
  pinMode(MS3, OUTPUT);
  pinMode(button_ForwardReverse, INPUT);
  pinMode(button_HaltStart, INPUT);
  pinMode(button_Microstep, INPUT);
}


void loop() {
  P1 = digitalRead(button_HaltStart);
  M01 = P1 && (M01 || !HaltStart) && !M02;
  M02 = P1 && (M02 || HaltStart) && !M01;
  HaltStart = (M01 || HaltStart) && !M02;

  if (digitalRead(button_ForwardReverse)){
    setForwardReverse();
  }
  
  P3 = digitalRead(button_Microstep);
  if (P3 != lastTrigMicrostep){
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (P3 != TrigMicrostep) {
      TrigMicrostep = P3;
      if (TrigMicrostep == HIGH) {
        Microstep++;
        Serial.println(Microstep);
        Microstep = Microstep%5;
        switch (Microstep) {
          case 1:
            digitalWrite(MS1, LOW);
            digitalWrite(MS2, LOW);
            digitalWrite(MS3, LOW);
            break;
          case 2:
            digitalWrite(MS1, HIGH);
            digitalWrite(MS2, LOW);
            digitalWrite(MS3, LOW);
            break;
          case 3:
            digitalWrite(MS1, LOW);
            digitalWrite(MS2, HIGH);
            digitalWrite(MS3, LOW);
            break;
          case 4:
            digitalWrite(MS1, HIGH);
            digitalWrite(MS2, HIGH);
            digitalWrite(MS3, LOW);
            break;
          case 5:
            digitalWrite(MS1, HIGH);
            digitalWrite(MS2, HIGH);
            digitalWrite(MS3, HIGH);
            break;
        }
      }
    }
  }

  lastTrigMicrostep = P3;
    
  digitalWrite(dirPin, ForwardReverse);
  digitalWrite(stepPin, HaltStart);
  delayMicroseconds(1000);
  digitalWrite(stepPin, LOW);
  delayMicroseconds(1000);

//  delay(1000);
}



void setForwardReverse(){
  digitalWrite(stepPin, LOW);
  delay(2000);
  M11 = HIGH && (M11 || !ForwardReverse) && !M12;
  M12 = HIGH && (M12 || ForwardReverse) && !M11;
  ForwardReverse = (M11 || ForwardReverse) && !M12;
  M11=0;
  M12=0;
  digitalWrite(stepPin, HIGH);
}

/*

MS1	  MS2	  MS3	  Microstep resolution
--------------------------------------
Low	  Low	  Low	  Full step
High  Low	  Low	  1/2 step
Low	  High	Low	  1/4 step
High	High	Low	  1/8 step
High	High	High	1/16 step


    driver A4988 e DRV8825
       ,-----------,
ENABLE |o         o| V_MOTORE
   MS1 |o ____    o| GND
   MS2 |o|    |   o| 2B
   MS3 |o|____|   o| 2A
 RESET |o         o| 1A
 SLEEP |o         o| 1B
  STEP |o         o| VDD
   DIR |o         o| GND
       `-----------'
*/