#include <TimerOne.h>

int one = A1;
int two = A2;
int three = A3;
int en = A5;

//int in = 2;

void setup() {
  // put your setup code here, to run once:

  //1000000 is 1.0 Second
    //2000 = 2ms
    //2000
   Timer1.initialize(100); // set a timer of length 100000 microseconds (or 0.1 sec - or 10Hz => the led will blink 5 times, 5 cycles of on-and-off, per second)
   Timer1.attachInterrupt( timerIsr ); // attach the service routine here

   //pinMode(in, INPUT_PULLUP);
   attachInterrupt(0, isr, FALLING);

   pinMode(one, OUTPUT);
   pinMode(two, OUTPUT);
   pinMode(three, OUTPUT);
   pinMode(en, OUTPUT);
   pinMode(13, OUTPUT);


   Serial.begin(9600);
}

bool seq[][3] = {
      {true, true, false },
      {true, false, false },
      {true, false, true },
      {false, false, true },
      {false, true, true },
      {false, true, false },
  };


volatile long msold = 0;
volatile long msnew = 0;

volatile int mpr1 = 0;
volatile int mpr2 = 0;

volatile long tick = 0;
volatile int freq = 250; //frequency at which every freq'nth time the ISR calls, we do next step
volatile int freqmin = 25; //the fastest theoretical speed is determined by this


void isr()   //magnet speed sensor
{
  msold = msnew; //set "old new" ms to oldms
  msnew = micros();

  mpr2 = mpr1;
  mpr1 = mpr();

  if(mpr1 < mpr2)
  {
      if(freq > 100)
          freq -= 10;
      else if(freq > 15)
       if(tick % 3 == 0)   freq -= 1;  //In order to get more speed I had to slow down the acceleration, hence the % 3
  }

  digitalWrite(13, !digitalRead(13));
}

long mpr() //return Millisecond Per Revolution
{
 return msnew - msold;
}

void timerIsr()
{
  tick++;
  if(tick % freq == 0)
      nextStep();
}

volatile long steps = 0;

volatile byte pointer = 0;

void nextStep()
{
  steps++;
   if(pointer > 5)
    {
        pointer = 0; //reset if we reached last step. happens 4 times a revolution;
    }

     digitalWrite(en, LOW); //disable ENABLE pin
     digitalWrite(one, seq[pointer][0]);
     digitalWrite(two, seq[pointer][1]);
     digitalWrite(three, seq[pointer][2]);
     digitalWrite(en, HIGH); //enable ENABLE pin
     pointer++;
}


void loop() {
  // put your main code here, to run repeatedly:

  delay(500); Serial.println(mpr());


}