#include <Stepper.h>

#define IN1  5
#define IN2  6
#define IN3  7
#define IN4  8
#define BTN_SETUP 2
#define IDLE_SIZE 8
#define LED_SETUP 12

// 28BYJ-48
// gear ratio is in fact 63.68395:1,
// which results in approximately 4076 steps per full revolution (in half step mode).
const int stepsPerRevolution = 64;  // change this to fit the number of steps per revolution
const int stepsInLoop = 2;

// initialize the stepper library on pins 8 through 11:

Stepper myStepper(stepsPerRevolution, IN1, IN2, IN3, IN4);
unsigned long printMilis = 0;
bool btnUpProcessed = false;
long motorPosition = 0;
long motorPositionMin = 0;
long motorPositionMax = 0;


void setup() {

  // nothing to do inside the setup
  Serial.begin(115200); // Any baud rate should work
  Serial.println("Init\n");

  pinMode(BTN_SETUP, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(LED_SETUP, OUTPUT);

  printMilis = millis();
  btnUpProcessed = false;
  motorPosition = 0;
  motorPositionMin = 0;
  motorPositionMax = 0;
}

void loop() {
  int sensorReading = analogRead(A0);
  int speed = abs(sensorReading - 512);
  int direction = (sensorReading < 512) ? -1 : 1;

  bool timeToPrint = false;
  bool motorSetupMode = (digitalRead(BTN_SETUP) == LOW);

  digitalWrite(LED_BUILTIN, motorSetupMode);
  digitalWrite(LED_SETUP, !motorSetupMode);

  if((millis() - printMilis) > 1000) {
    printMilis = millis();
    timeToPrint = true;
  }

  if(motorSetupMode) {
    // Motor setup
    if(!btnUpProcessed) {
      Serial.println("Start motor setup");
      btnUpProcessed = true;
      motorPositionMin = motorPosition;
      motorPositionMax = motorPosition;
    }
  } else {
    btnUpProcessed = false;
  }

  if(speed < IDLE_SIZE){
    // Idle - do nothing
    delay(10);

    if(timeToPrint) {
      Serial.print("IDLE");
    }

  } else {
    myStepper.setSpeed(map(speed, 0, 512, 0, 16)); // Set speed as RPMS

    if(timeToPrint) {
      Serial.print(direction > 0 ? "+" : "-");
      Serial.print(speed);
    }

    // step 1/100 of a revolution
    //myStepper.step((direction * stepsPerRevolution) / 100);

    if(
      motorSetupMode 
      || (direction > 0 && (motorPosition + stepsInLoop) <= motorPositionMax)
      || (direction < 0 && (motorPosition - stepsInLoop) >= motorPositionMin)
    ) {
      // Allowed motor step
      myStepper.step(direction * stepsInLoop);
      motorPosition += (direction * stepsInLoop);

      // Update new position
      if(motorSetupMode) {
        if( motorPosition < motorPositionMin) {
          motorPositionMin = motorPosition;
        }
        if(motorPositionMax < motorPosition) {
          motorPositionMax = motorPosition;
        }
      }
    }    
  }

  

  if(timeToPrint) {
    Serial.print(" (p: ");
    Serial.print(motorPosition);
    Serial.print(",min: ");
    Serial.print(motorPositionMin);
    Serial.print(",max: ");
    Serial.print(motorPositionMax);
    Serial.print(" ) ");

    if(motorSetupMode) {
      Serial.print(" - setup mode ");
    }

    Serial.println("");
  }
}