#include <AccelStepper.h>

// Imposta i pin del driver A4988
#define STEP_PIN 3
#define DIR_PIN 6
#define button_HaltStart 4
#define button_ForwardReverse 5
#define button_Microstep 2

int MS[3] = {9, 8, 7}; // pin microstepper MS1=MS[0]->9,
                       // MS2=MS[1]->8, MS3=MS[2]->7

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

int Microstep,speed = 0;
long debouncingTime = 15;
volatile unsigned long lastMusec;

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

// Crea un oggetto stepper
AccelStepper stepper(1, STEP_PIN, DIR_PIN);

void setup() {
  Serial.begin(115200);
  // Imposta la velocità massima e l'accelerazione
  stepper.setMaxSpeed(1000);
  stepper.setAcceleration(500);

  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(MS[0], OUTPUT);
  pinMode(MS[1], OUTPUT);
  pinMode(MS[2], OUTPUT);
  pinMode(button_HaltStart, INPUT);
  pinMode(button_Microstep, INPUT);
  attachInterrupt(0, debounce, RISING);

  for (int step = 0; step < 3; step++) {
      digitalWrite(MS[step],microstepArray[0][step]);
      Serial.println(microstepArray[0][step]);
  }
}

void loop() {

  P1 = digitalRead(button_HaltStart);
  M01 = P1 && (M01 || !HaltStart) && !M02;
  M02 = P1 && (M02 || HaltStart) && !M01;
  HaltStart = (M01 || HaltStart) && !M02;
  //Serial.print(HaltStart);

  if (HaltStartNEW != HaltStart && HaltStart == 1) {
    speed = 2000;
    HaltStartNEW = HaltStart;
  }
  else if (HaltStartNEW != HaltStart && HaltStart == 0) {
    speed = 0;
    HaltStartNEW = LOW;
  }

  stepper.setSpeed(speed);
  stepper.runSpeed();

  

  // Fai girare il motore in un senso
/*  if (HaltStart) {
    stepper.setSpeed(4000);
    stepper.runSpeed();
  }
  else if (HaltStart) {
    stepper.setSpeed(0);
    stepper.runSpeed();
  }
*/

  // Pausa
//  delay(5000);

  // Fai girare il motore nell'altro senso
 // stepper.setSpeed(-200);
 // stepper.runSpeed();

  // Pausa
//  delay(5000);
}


void debounce() {
  if((long)(micros() - lastMusec) >= debouncingTime*15000) {
    microstepInterrupt();
    lastMusec = micros();
  }
}

void microstepInterrupt() {
  if (digitalRead(button_Microstep)){
    Microstep++;
//    Serial.println(Microstep);
    Microstep = Microstep%5;
    for (int step = 0; step < 3; step++) {
      digitalWrite(MS[step],microstepArray[Microstep][step]);
      Serial.print(microstepArray[Microstep][step]);
    }
    Serial.println("");
  }
}