#include <DigitalSignals.h>     //https://github.com/cotestatnt/DigitalSignal
#include "StepperTNT.h"

#define BTN1 2
#define BTN2 3
#define DIR 5
#define STEP 6

#define SPD_PIN A0
#define ACC_PIN A1

#define MICROSTEPPING       4       // 1/4 di step
#define MIN_SPEED           100
#define MAX_SPEED           6000
#define MIN_ACCEL           1000
#define MAX_ACCEL           20000

StepperTNT  stepper(DIR, STEP);
DigitalIn   button1(BTN1, INPUT_PULLUP, 50);    // INPUT (pull-up) debounce threshold 50ms
DigitalIn   button2(BTN2, INPUT_PULLUP, 50);    // INPUT (pull-up) debounce threshold 50ms

uint32_t target, oldTarget;
const uint16_t pos1 = 15;
const uint16_t pos2 = 360;

uint16_t lastSpdVal  = 0;
uint16_t lastAccVal    = 0;

// Stampa il numero del GPIO associato al pulsante ed il momento in cui è stato premuto
void printButtonInfo(DigitalIn* inp) {
  Serial.print("Input (gpio #");
  Serial.print(inp->getPin());
  Serial.print(") active at ");
  Serial.print(millis());
  Serial.println(" ms.");
}

// Funzione di callback associata a button1
void moveToPos1(void* arg) {
  // Recupero il puntatore alla classe del pulsante che ha generato l'evento
  DigitalIn* input = (DigitalIn*) arg;
  // Stampo alcune informazioni di debug
  printButtonInfo(input);
  // Imposto la nuova posizione
  target = pos1 * MICROSTEPPING;
}
// Funzione di callback associata a button2
void moveToPos2(void* arg) {
  // Recupero il puntatore alla classe del pulsante che ha generato l'evento
  DigitalIn* input = (DigitalIn*) arg;
  // Stampo alcune informazioni di debug
  printButtonInfo(input);
  // Imposto la nuova posizione
  target = pos2 * MICROSTEPPING;
}

// interrupt service routine wrapper
ISR(TIMER1_OVF_vect) {
  stepper.isrCallback();
}

void setup() {
  Serial.begin(115200);
  stepper.setSpeed(2000);
  stepper.setAcceleration(4000);

  button1.onPressed(moveToPos1);
  button2.onPressed(moveToPos2);
}

void loop () {
  // Aggiorno lo stato dei pulsanti
  button1.update();
  button2.update();

  // Leggo lo stato dei pulsanti tenendo conto di un minimo di rumore
  if (abs(analogRead(SPD_PIN) - lastSpdVal) > 5) {
    lastSpdVal = analogRead(SPD_PIN);
    uint16_t speed = map(lastSpdVal, 0, 1023, MIN_SPEED, MAX_SPEED);
    stepper.setSpeed(speed);
    Serial.print("Velocità: ");
    Serial.println(speed);
  }

  if (abs(analogRead(ACC_PIN) - lastAccVal) > 5) {
    lastAccVal = analogRead(ACC_PIN);
    uint16_t accel = map(lastAccVal, 0, 1023, MIN_ACCEL, MAX_ACCEL);
    stepper.setAcceleration(accel);
    Serial.print("Accelerazione: ");
    Serial.println(accel);
  }

  // Se è stata impostata una nuova posizione da raggiungere
  if (target != oldTarget) {
    oldTarget = target;

    // Imposto il target (numero di step da eseguire)
    stepper.moveTo(target);
    // Avvio del movimento
    stepper.start();
  }

}