// Define stepper motor connections and steps per revolution:
#define dirPin 9
#define stepPin 8
#define stepsPerRevolution 200
#define stepsPerMM 40

#define SLEEPING 0
#define CHECKING 1
#define MATLAB 2
#define HOMING 3
#define ACTIVATIONDISTANCE 700
#define MAXIMUMDISTANCE 1400

boolean DEBUG = false;

byte PWM_PIN = 6;
double distance;
int systemStatus = SLEEPING;
int currentDistance = 0;
boolean Dir = LOW;

void setup()
{
  // Declare pins as output:
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(PWM_PIN, INPUT);

  
  Serial.begin(9600);
  delay(10000);
}
void loop()
{
     readDistance();
           
      if (DEBUG) 
      Serial.println("SLEEPING");
      
     if(systemStatus == SLEEPING && distance < ACTIVATIONDISTANCE){

      systemStatus = CHECKING;
      }
      
      else if(systemStatus == CHECKING){
        

      
      while(distance < ACTIVATIONDISTANCE){
        MovePlate(10,LOW);
        readDistance();
      if (DEBUG) 
      Serial.println("CHECKING");
      }
      
      while(distance > ACTIVATIONDISTANCE){
        MovePlate(10,HIGH);
        readDistance();
      if (DEBUG) 
      Serial.println("CHECKING");
      }
      
      MovePlate(100,HIGH); //offset value
      
      systemStatus = MATLAB;
      }

      else if(systemStatus == MATLAB){
        

        Serial.print("1\n");
        while(distance < ACTIVATIONDISTANCE){
        readDistance();
        if (DEBUG) 
        Serial.println("MATLAB");
        }
        systemStatus = HOMING;
      }

      else if(systemStatus == HOMING){

        if (DEBUG) 
        Serial.println("HOMING");
        
       Serial.print("0\n");
       MovePlate(currentDistance,HIGH);
       systemStatus = SLEEPING;
      }
      
  delay(50);
 }


 void MovePlate(double j, boolean d)
 {
  digitalWrite(dirPin, d);
  if (d == LOW){
    currentDistance+=j;
  }
  else {
    currentDistance-=j;
  }

 if (currentDistance < MAXIMUMDISTANCE){
  if (systemStatus == HOMING or d == HIGH){
        for(double i = 0; i < j*stepsPerMM; i++)
        {
          // These four lines result in 1 step:
          digitalWrite(stepPin, HIGH); 
          delayMicroseconds(100); //100 is default
          digitalWrite(stepPin, LOW);
          delayMicroseconds(100); //100 is default
        }
    
  }
  else{
    
  int steppingDelay = 600;
  int varSpeed = 0;
  
  for(double i = 0; i < j*stepsPerMM; i++)
  {
    // These four lines result in 1 step:
    digitalWrite(stepPin, HIGH); 
    if ( j > 10)
    delayMicroseconds(steppingDelay); //100 is default

    else  delayMicroseconds(100); //100 is default 
    
    digitalWrite(stepPin, LOW);
    if (j > 10)
    delayMicroseconds(steppingDelay); //100 is default 

    else  delayMicroseconds(100); //100 is default 

    if (varSpeed < 6 && j > 10){
        varSpeed++;
        steppingDelay -= 100;
    }
   
    else if ( ((j*stepsPerMM)-i) < 6 ){
        steppingDelay += 100;
    }

  }
    }
  }
 }

 void readDistance(){
     distance = pulseIn(PWM_PIN, HIGH);
     distance = (distance)/10;
     if (DEBUG)
     Serial.println(distance);
     Serial.println(currentDistance);
     delay(100);
 }