/*
 * Arduino proMicro and DRV8834
 * 
 * M0     M1    Stepper resolution
 * ---------------------------
 * Low    Low   Full step
 * High   Low   Half step
 * Float  Low   1/4 step
 * Low    High  1/8 step
 * High   High  1/16 step
 * Float  High  1/32 step
 * 
 * Interrupts on board :
 * pin 3 is interrupt 0
 * pin 2 is interrupt 1
 * pin 0 is interrupt 2
 * pin 1 is interrupt 3
 * pin 7 is interrupt 4
 * 
 * Used interrupt : pin 2 is interrupt 1
 */

#include <AccelStepper.h>
#include <EEPROM.h>

const int _ENABLE= 21;  // _ENABLE, active low
const int M0     = 20;  // See table in header..
const int M1     = 19;  // 
const int _SLEEP = 14;  // _SLEEP, active low
const int STEP   = 16;  // Stepper pin
const int DIR    = 10;  // Direction pin
const int SENSOR = 2;   // Hall-sensor for pos 1

word filterPos[]={ 0, 0, 1288, 2576, 3864, 5152, 6440 };
word posOffset[]={ 0, 90, 72, 72, 72, 72 };
bool Error = false;                      // Error flag
String inLine;                           // Current command.
char command;                            // Command
byte currPos = 1;                        // Start up with 1
int newPos = 1;                          // Just start somewhere
bool cmdOK = false;                      // Command ok ?
int PWMvalue = 128;                      // Set PWM to half
unsigned long timeToFlagError;
const unsigned long maxWaitTime = 15000; // Waittime for slot if not found
volatile byte revolutionCounter = 0;     //

AccelStepper stepper(1, STEP, DIR);

void setup() {

Serial.begin(9600);
while ( !Serial ) {
  // Wait until Serial is available.
}
Serial.flush();

// Define pinmodes.
pinMode(_ENABLE, OUTPUT);
pinMode(M0, OUTPUT);
pinMode(M1, OUTPUT);
pinMode(_SLEEP, OUTPUT);
pinMode(STEP, OUTPUT);
pinMode(DIR, OUTPUT);
pinMode(SENSOR, INPUT_PULLUP);

// Set pins. !! Pins are inverted !!
pinMode(_ENABLE, LOW);
pinMode(M0, LOW);
pinMode(M1, HIGH);
pinMode(_SLEEP, HIGH);
pinMode(STEP, LOW);
pinMode(DIR, LOW);

// Set stepper stuff
stepper.setCurrentPosition(0);
stepper.setMaxSpeed(6000);
stepper.setSpeed(1000);
stepper.setAcceleration(5000);     // For runspeed() or run() ??

stupidInit();                      // Init and find slot 1.
}

void loop() {

// Get incoming command.
  Serial.setTimeout(250);
  inLine=Serial.readStringUntil('\n');


// Run debugProcedure..
  if ( inLine == "G0" ) {
    cmdOK = true;
    debugProcedure();  // Go do some debugging..
    return;
  }

////////////////////////////////////////////////////////////////////////////
  if ( inLine == ")" ) {
    cmdOK = true;
    inLine=")0";
  }


  if ( inLine == "(" ) {
    cmdOK = true;
    inLine="(0";
  }
////////////////////////////////////////////////////////////////////////////

// Take care of commands G1 - G5
  command = inLine.charAt(0);         // command+newPos=Goto pos newPos
  newPos = int(inLine.charAt(1)-48);  // newPos=int -48 if no input..

  if ( newPos == -48 ) {
    return;
  }

  if ( command == 'G' && ( newPos >= 1 || newPos <= 5 ) ) {

    if ( newPos == 1 ) {
      Locate_Slot_1();
      }

    if ( newPos > 5 ) {
      newPos = currPos;
      }

    if ( newPos != currPos ) {
      Locate_Slot_x();
    }
  Serial.print("P");
  Serial.println(currPos);
  }


// Hard reboot, do same as R1.
  if ( inLine == "R0" ) {
    cmdOK = true;
    Locate_Slot_1();  // currPos will be 1.
    delay(1000);
    Serial.print("P");
    Serial.println(currPos);
  }


// Initialize, restarts and moves to filter position 1.
  if ( inLine == "R1" ) {
    cmdOK = true;
    Locate_Slot_1();  // currPos will be 1.
    // currPos = 1;
    delay(1000);
    Serial.print("P");
    Serial.println(currPos);
  }


// Reset all calibration values to 0.
  if ( inLine == "R2" ) {
    cmdOK = true;
    filterPos[0]=0;
    filterPos[1]=0;
    filterPos[2]=0;
    filterPos[3]=0;
    filterPos[4]=0;
    filterPos[5]=0;
    filterPos[6]=0;
    delay(1000);
    Serial.println("Calibration Removed");
  }


// Reset Jitter value to 1, displays "Jitter 1"
  if ( inLine == "R3" ) {
    cmdOK = true;
    Serial.println("Jitter 5");
  }


// Reset maximum carousel rotation speed to 100%, displays "MaxSpeed 100%"
  if ( inLine == "R4" ) {
    cmdOK = true;
    Serial.println("MaxSpeed 100%");
  }


// Reset Threshold value to 30, displays "Threshold 30"
  if ( inLine == "R5" ) {
    cmdOK = true;
    Serial.println("Threshold 30");
  }


// Calibrate, No return value displayed.
  if ( inLine == "R6" ) {
    cmdOK = true;
    delay(1000);
  }


// Product name
  if ( inLine == "I0" ) {
    cmdOK = true;
    Serial.println("Xagyl FW5125V1");
  }


// Firmware version
  if ( inLine == "I1" ) {
    cmdOK = true;
    Serial.println("3.1.5");
  }


// Current filter pos
  if ( inLine == "I2" ) {
    cmdOK = true;
    Serial.print("P");
    Serial.println(currPos);
  }


// Serial number
  if ( inLine == "I3" ) {
    cmdOK = true;
    Serial.println("Arduino Xagyl Interface v5.0sm");
  }


// Display the maximum rotation speed - "MaxSpeed XXX%"
  if ( inLine == "I4" ) {
    cmdOK = true;
    Serial.println("MaxSpeed 100%");
  }


// Display the jitter value - "Jitter XX", XX = values 1-10
  if ( inLine == "I5" ) {
    cmdOK = true;
    Serial.println("Jitter 5");
  }

//////////////////////////////////////////////////////////////////////////////
// Display sensor position offset for current filter position - "PX Offset XX"
  if ( inLine == "I6" ) {
    cmdOK = true;
    Serial.print("P");
    Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[currPos] );
  }
//////////////////////////////////////////////////////////////////////////////

// Display filter position sensor threshold value - "Threshold XX"
  if ( inLine == "I7" ) {
    cmdOK = true;
    Serial.println("Threshold 30");
  }


// Display the number of available filter slots - "FilterSlots X"
  if ( inLine == "I8" ) {
    cmdOK = true;
    Serial.println("FilterSlots 5");
  }


// Display the Pulse Width value - "Pulse Width XXXXXuS" : PWMvalue 0..255
  if ( inLine == "I9" ) {
    cmdOK = true;
    Serial.print("Pulse Width ");  // Default 1500uS, Range 100 - 10000
    Serial.println("4950uS");      // 10000-100/2=4950 Just a value
  }


// Display sensor position offset for filter position Value - "PX Offset XX"
  if ( inLine == "O1" ) {
    cmdOK = true;
    Serial.print("P1");
    //Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[1] );
  }


// Display sensor position offset for filter position Value - "PX Offset XX"
  if ( inLine == "O2" ) {
    cmdOK = true;
    Serial.print("P2");
    //Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[2] );
  }


// Display sensor position offset for filter position Value - "PX Offset XX"
  if ( inLine == "O3" ) {
    cmdOK = true;
    Serial.print("P3");
    //Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[3] );
  }


// Display sensor position offset for filter position Value - "PX Offset XX"
  if ( inLine == "O4" ) {
    cmdOK = true;
    Serial.print("P4");
    //Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[4] );
  }


// Display sensor position offset for filter position Value - "PX Offset XX"
  if ( inLine == "O5" ) {
    cmdOK = true;
    Serial.print("P5");
    //Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[5] );
  }


// Hall-sensor data..
  if ( inLine == "T0" ) {
    cmdOK = true;
    Serial.print("Sensors ");
    Serial.print(digitalRead(SENSOR));
    Serial.print(" ");
    Serial.println(digitalRead(SENSOR));
  }


// Hall-sensor data..
  if ( inLine == "T1" ) {
    cmdOK = true;
    Serial.print("Sensors ");
    Serial.print(digitalRead(SENSOR));
    Serial.print(" ");
    Serial.println(digitalRead(SENSOR));
  }


// Hall-sensor data..
  if ( inLine == "T2" ) {
    cmdOK = true;
    Serial.println("MidRange 520");
  }


// Hall-sensor data..
  if ( inLine == "T3" ) {
    cmdOK = true;
    Serial.print("RightCal ");
    Serial.println(digitalRead(SENSOR) - digitalRead(SENSOR));
  }

////////////////////////////////////////////////////////
// Go clockwise...
  if ( inLine == ")0" ) {
    cmdOK = true;
//    delay(50);
    posOffset[currPos] = posOffset[currPos] + 1;
    /**/
    pinMode(DIR, LOW);                               // Go clockwise
    stepper.setCurrentPosition(0);                   // Set new zero-point including offset
    digitalWrite(_SLEEP, HIGH);                      // Wake up..
    stepper.runToNewPosition( posOffset[currPos] );  // Run to offset of currPos
    digitalWrite(_SLEEP, LOW);                       // Goto sleepmode
    /**/
    Serial.print("P");
    Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[currPos] );
    return;
  }


// Go counterclockwise...
  if ( inLine == "(0" ) {
    cmdOK = true;
    posOffset[currPos] = posOffset[currPos] - 1;
    /**/
    pinMode(DIR, HIGH);                              // Go counterclockwise
    stepper.setCurrentPosition(0);                   // Set new zero-point including offset
    digitalWrite(_SLEEP, HIGH);                      // Wake up..
    stepper.runToNewPosition( posOffset[currPos] );  // Run to offset of currPos
    digitalWrite(_SLEEP, LOW);                       // Goto sleepmode
    /**/
    Serial.print("P");
    Serial.print(currPos);
    Serial.print(" Offset ");
    Serial.println( posOffset[currPos] );
    return;
  }
////////////////////////////////////////////////////////

// Increase pulse width by 100uS, Displays “Pulse Width XXXXXuS”
  if ( inLine == "M0" ) {
    cmdOK = true;
    PWMvalue = PWMvalue + 100;
    Serial.print("Pulse Width ");
    Serial.print(PWMvalue);
    Serial.println("uS");
  }


// Decrease pulse width by 100uS, Displays “Pulse Width XXXXXuS”
  if ( inLine ==  "N0" ) {
    cmdOK = true;
    PWMvalue = PWMvalue - 100;
    Serial.print("Pulse Width ");
    Serial.print(PWMvalue);
    Serial.println("uS");
  }


// Decrease filter position threshold value, Displays Displays "Threshold XX"
  if ( inLine ==  "{0" ) {
    cmdOK = true;
    Serial.println("Threshold 30");
  }


// Increase filter position threshold value, Displays Displays "Threshold XX"
  if ( inLine ==  "}0" ) {
    cmdOK = true;
    Serial.println("Threshold 30");
  }


// Decrease jitter window by 1, Displays Displays “Jitter X” value 1-10.
  if ( inLine ==  "[0" ) {
    cmdOK = true;
    Serial.println("Jitter 5");
  }


// Increase jitter window by 1, Displays Displays “Jitter X” value 1-10.
  if ( inLine ==  "]0" ) {
    cmdOK = true;
    Serial.println("Jitter 5");
  }


// If command not recognized, flush buffer and wait for next command..
  if ( cmdOK ) {
    Serial.flush();
    cmdOK = false;
  }
}   ////////// End of main loop.. //////////


// For some reason this is needed.
void stupidInit() {
int cnt;
word apa;

for ( cnt = 0; cnt != 7; cnt = cnt + 1 ) {
  apa=posOffset[cnt];
  apa=filterPos[cnt];
  }

Locate_Slot_1();  // Just find slot 1
}


// Try to find slot 1 *******************************************************
void Locate_Slot_1() {

revolutionCounter = 0;
attachInterrupt(digitalPinToInterrupt(SENSOR), Interrupter, FALLING);

// Move to filterPos 1.
stepper.setCurrentPosition(0);
digitalWrite(_SLEEP, HIGH);    // Wakeup...
stepper.moveTo( 28980 );       // Ett helt varv : 6440
//stepper.moveTo( 1288 );      // Ett filter    : 1288

while ( stepper.distanceToGo() > 0 ) {
  if ( revolutionCounter == 2 ) {
    stepper.setCurrentPosition(0);
    stepper.stop();
  }
  stepper.run();
}

stepper.runToNewPosition( posOffset[currPos] );  // Run to offset, depends on magnet pos.
stepper.setCurrentPosition(0);                   // Set new zero-point including offset

//Serial.println( posOffset[currPos] );

detachInterrupt(digitalPinToInterrupt(SENSOR));  // Remove hook to interrupt

digitalWrite(_SLEEP, LOW);                       // Goto sleepmode

//Serial.println( currPos );
//Serial.println( posOffset[currPos] );

currPos = 1;                                     // Tell caller this is slot 1.
return;
}


// Try to find slot 2..5 ***************************************************
void Locate_Slot_x() {

if ( filterPos[newPos] < filterPos[currPos] ) {
    stepper.moveTo( -stepper.currentPosition() );
  }

stepper.moveTo( filterPos[newPos] );
digitalWrite( _SLEEP, HIGH );

while ( stepper.distanceToGo() != 0 ) {
    stepper.run();
  }

//stepper.setCurrentPosition(0);
stepper.moveTo( filterPos[newPos] );          // Select filterPos[newPos] from array
    while ( stepper.distanceToGo() > 0 ) {
        stepper.run();
    }

digitalWrite(_SLEEP, LOW);    // Goto sleepmode
currPos = newPos;             // Tell caller this is the new requested position
return;
}

// Calibrate wheel command : R6
void slotCalibrate() {
  delay(5000);
  return;
}


// Show some values and reset error flag.
void debugProcedure() {
int i=0;
word Value;

//EEPROM.write(address, value);
for ( i=0; i < 6 ; i++ ){
  EEPROM.write((i*2)+50, highByte(filterPos[i]));
  EEPROM.write((i*2)+51, lowByte(filterPos[i]));
}
Serial.println("Default values written to EEPROM!!");

Serial.println("\n");
Serial.println("-------------------------------");

Serial.print("Current Offset : ");
Serial.println( posOffset[currPos] );
Serial.println();

Serial.println("Values per memorycell");
for ( i=0; i < 6 ; i++ ){
  Value = word( EEPROM.read(i*2+50), EEPROM.read(i*2+51));
  Serial.print("Pos : ");
  Serial.print( i );
  Serial.print(" : ");
  Serial.println( Value );
}

Serial.println("");
Serial.print("Bytes in EEPROM : ");
Serial.println(EEPROM.length());
Serial.println("");

attachInterrupt(digitalPinToInterrupt(SENSOR), Interrupter, FALLING);

if ( !Error ) { Serial.println("Normal operation resumed");
}

Error = false;
return;
}

// Interrupt service routine
void Interrupter() {

  revolutionCounter++;

}