/*################### File version: 4.0 ##################*/
/*########################################################*/
/* Controllo stepmotor per il nastro trasportatore a      */
/* scopo didattico (il docente dovrà spiegarne il         */
/* funzionamento).                                        */
/* vengono utilizzati tre pulsanti:                       */
/*  - pulsante rosso P1 per halt e stop del motore;       */
/*  - pulsante verde P2 per controllare il senso          */
/*    di rotazione;                                       */
/*  - pulsante grigio per controllare il microstepping    */
/*    (quindi la velocità) del motore.                    */
/* Il programma utilizza per il tasto P1 tre funzioni     */
/* logiche con cui si tenta di simulare il comportamento  */
/* di un debounce hardware basato su flip flop; per il    */
/* tasto P2 la chiamata a funzione dove con tre funzioni  */
/* logiche simili a quelle per il tasto P1; per il tasto  */
/* P2 l'interrupt del microcontrollore.                   */
/*                                                        */
/*     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                              */
/*         `-----------'                                  */
/*                                                        */
/*    ==========================================          */
/*     MS1 | MS2	|  MS3 |  stepLevel 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                     */
/*    =========================================           */
/*########################################################*/
//##### urutils.h #####
void waitUntilInputLow(int btn, unsigned t)
{
   do{
	 delay(t);
   }while(digitalRead(btn)!=LOW);
}

struct DiffTimer
{
	volatile unsigned long elapsed, last;
	volatile bool timerState=false;
	void reset(){
	    elapsed = 0;
	    last = millis();
	}
	void toggle(){
	    if(timerState){
    	        stop();
	    }else{
		start();
	    }	
	}
	void stop(){
	    if(timerState){
		timerState = false;
    	        elapsed += millis() - last;
	    }	
	}
	void start(){
	    if(!timerState){
		timerState = true;
		last = millis();
	    }
	}
	unsigned long get(){
	    if(timerState){
		return millis() - last + elapsed;
	    }
	    return elapsed;
	}
	void set(unsigned long e){
	    reset();
	    elapsed = e;
	}
};
//##### urutils.h #####
#include <TimerOne.h>  // Libreria per gestire interrupt basati su timer
#define dirPin 6 //2
#define stepPin 3
#define button_ForwardReverse 5
#define button_HaltStart 4
#define button_Microstep 2 //6
#define  DEBOUNCETIME 50

volatile bool dirChange = false;
volatile bool directionChange;
volatile bool direction = true;
volatile bool stato = false;
volatile bool stepChange = false;
volatile int count1 = 0;
DiffTimer deb;
uint16_t speed;
uint8_t startStopPin = 0;
uint8_t stepLevel = 0;
uint16_t stepInterval = 1200; // Microsecond (600µs HIGH + 600µs LOW)

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

enum resolution{
	FULL,
	HALFT,
	QUARTER,
	EIGHTH,
	SIXTEENTH
};

// Definizione per lo stato dei pulsanti
struct Button {
  uint8_t pin;
  uint8_t val0;
  uint8_t val;
  //uint8_t count;
  //bool state;
  bool changed() {
	val = digitalRead(pin);
	bool changed = (val != val0);
	val0 = val;	// aggiornamento livello precedente al livello attuale
	return changed;     // ritorna il valore attuale del pulsante
  }
};

Button buttonStart = {button_HaltStart, LOW};
Button buttonDir = {button_ForwardReverse, LOW};
Button buttonStep = {button_Microstep, LOW};

bool setResolution(uint8_t level){
	if (level < 0 || level > 4) return false;
	Serial.print("Res: ");
	for (int i = 0; i < 3; i++) {
      digitalWrite(MS[i],microstepArray[level][i]);
      Serial.print(microstepArray[level][i]);
    }
	Serial.println();
	return true;
}

// Funzione di interrupt per controllare gli step del motore
void stepMotor() {
  static bool stepState = false;
  
  if (directionChange) {
    return;  // Non generare step durante il cambio di direzione
  }
  
  stepState = !stepState;
  digitalWrite(stepPin, stepState);
}

void setup() {
  Serial.begin(115200);
 
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(MS[0], OUTPUT);
  pinMode(MS[1], OUTPUT);
  pinMode(MS[2], OUTPUT);
  pinMode(button_ForwardReverse, INPUT);
  pinMode(button_HaltStart, INPUT);
  pinMode(button_Microstep, INPUT);
   
  setResolution(FULL);
  stato = false;
  speed = 600;
  direction = true;
  directionChange = false;
  deb.start();
  
  // Configura l'interrupt di timer per gli step del motore
  Timer1.initialize(stepInterval/2);  // Metà del periodo totale
  Timer1.attachInterrupt(stepMotor);
  Timer1.stop();  // Ferma il timer finché non è necessario
}

void loop() {
	
	if(deb.get() >= DEBOUNCETIME) {                       
		deb.reset();   
	
		if(buttonStep.changed()){
			if(buttonStep.val){
			  stepLevel++;
			  stepLevel = stepLevel%5;
			  setResolution(stepLevel);
			}
		}

		if(buttonDir.changed()){
		  //digitalWrite(stepPin, LOW);
		  directionChange = true;
			Serial.print("Direction: ");Serial.println(buttonDir.val);
		  delay(1000);
		  digitalWrite(dirPin, buttonDir.val);
		  directionChange = false;
		  //digitalWrite(stepPin, HIGH);
		}

		if(buttonStart.changed()){
			if(buttonStart.val){
				Timer1.start();
				Serial.println("Timer started");
			}else{
				Timer1.stop();
				Serial.println("Timer stopped");
			}
		}
	}
  delay(1);
}