#include <Servo.h>
#include <Stepper.h>

const int dirPin = 2;
const int stepPin = 3;
const int LswitchUp = 4;
const int LswitchDown = 5;
const int swPin = 7;
const int servoPin = 9;
const int ledRed = 12; // sw_on = ture;
const int ledWhite = 13; // sw_on = false; with Uno #13 LED

Servo myservo; 

// change this to the number of steps on your motor
#define STEPS 200

Stepper stepper(STEPS, dirPin, stepPin);

int sw = 1; // switch value (default:1 = off)
int sv_pos = 0; // currrent servo position
int step_pos = 0; // current steps position
int x = 0; // x steps per loop

boolean sw_on = false; // current switch status
boolean dir_up = true; // current direction

boolean edgeUp = false; 
boolean edgeDown = false; 

int steps_begin = 0;
int steps_home = 200;
int steps_end = 3600; // steps_end is measured by setup()
int steps_turn = 3400; // steps_turn is calculated after steps_end is measured. 

void lightLED(){
  if (sw_on){
    digitalWrite(ledWhite, LOW);
    digitalWrite(ledRed, HIGH);
  } else {
    digitalWrite(ledWhite, HIGH);
    digitalWrite(ledRed, LOW);
  }
}

// Get back from current step position to home
void back_to_home(){
  Serial.print("step_pos: ");
  Serial.println(step_pos);
  Serial.print("dir_up: ");
  Serial.println(dir_up);

  // Homing stepper
  if (step_pos != steps_home) {
    stepper.step(-step_pos + steps_home);
    step_pos = steps_home;
  }
  delay(1000);
}

// This is called if Limit switch in edgeUp ON (LOW). 
// Return to home (by touching edgeDown)
void edgeUp_to_home(){
  Serial.println("edgeUp to home()"); 
  Serial.print("step_pos: ");
  Serial.println(step_pos);
  
  // Step down to edgeDown
  while (true) {
    stepper.step(-x);
    step_pos = step_pos -x;

    //  Limit switch in edgeDown ON (LOW), stop stepper
    if( (digitalRead(LswitchDown) == LOW) && (edgeDown == false) ) {
      break;
    }
  }
  // go to home
  edgeDown_to_home();
}

// This is called if Limit switch in edgeDown ON (LOW). 
// Return to home
void edgeDown_to_home(){
  Serial.println("edgeDown to home()"); 
  Serial.print("edgeDown: ");
  Serial.println(step_pos);
  
  // Step up to home
  dir_up = true;
  stepper.step(steps_home);
  step_pos = steps_home;

  Serial.print("to home: ");
  Serial.println(step_pos);
}


void setup(){
  pinMode(swPin, INPUT_PULLUP);  
  pinMode(dirPin, OUTPUT);  
  pinMode(LswitchUp, INPUT_PULLUP); 
  pinMode(LswitchDown, INPUT_PULLUP); 
  
  myservo.attach(servoPin);

  // set the speed of the motor to 500 RPMs
  stepper.setSpeed(500);
  x = STEPS/100;
  Serial.begin(115200);

  // Step up until detect collision to edgeUp
  while (true) {
    stepper.step(x);
    step_pos = step_pos + x;
    
    // If Limit switch UP is hit
    if( digitalRead(LswitchUp) == LOW) {
      Serial.println("edgeUp collision detected");
      break;
    }
  }
  steps_end = step_pos;
  Serial.print("Detected step_end position (edgeUp) is: ");
  Serial.println(steps_end);

  // Step down until detect collision to edgeDown
  while (true) {
    stepper.step(-x);
    step_pos = step_pos - x;
    
    // If Limit switch UP is hit
    if( digitalRead(LswitchDown) == LOW) {
      Serial.println("edgeUp collision detected");
      break;
    }
  }

  steps_begin = step_pos;
  Serial.print("Detected steps_begin position (edgeDown) is: ");
  Serial.println(steps_begin);

  steps_end = steps_end - steps_begin;
  steps_begin = 0;
  steps_turn = steps_end - 200;
  steps_home = 200;
  step_pos = 0;
  
  Serial.println("---Adjusted steps positons---" );
  Serial.print("steps_begin: ");
  Serial.println(steps_begin);
  Serial.print("steps_home: ");
  Serial.println(steps_home);
  Serial.print("steps_turn: "); 
  Serial.println(steps_turn);
  Serial.print("steps_end: ");
  Serial.println(steps_end);
  Serial.println("------------------------------" );
  
  edgeDown_to_home();
}

void loop(){
  
  // Check switch and toggle
  sw = digitalRead(swPin);

  // Toggle LED only when sw_on status is changed
  if ((sw == 0 ) && (sw_on == false)){
    sw_on = true;
    lightLED();
    Serial.println("Switch ON");
    delay(500);
  
  } else if ((sw == 0 ) && (sw_on )){
    sw_on = false;
    lightLED();
    Serial.println("Switch OFF");
    delay(1000);
  }

  // If Limit switch UP is hit
  if( (digitalRead(LswitchUp) == LOW) && (edgeUp == false) ) {
    Serial.println("edgeUp collision detected"); 
    edgeUp = true; 

    // Return to home and stop
    edgeUp_to_home();
    sw_on = false;


  } else if( (digitalRead(LswitchUp) == HIGH) && (edgeUp == true) ) {
    edgeUp = false;
    delay(20); 
  }

  // If Limit switch Down is hit
  if( (digitalRead(LswitchDown) == LOW) && (edgeDown == false) ) {
    Serial.println("edgeDown collision detected"); 
    edgeDown = true; 

    // Return to home and stop
    edgeDown_to_home();
    dir_up = true;
    sw_on = false;

  } else if( (digitalRead(LswitchDown) == HIGH) && (edgeDown == true) ) {
    edgeDown = false;
    delay(20); 
  }


  // Switch ON
  if (sw_on) {

    // Check steps, rotate servo and toggle direction
    // only on steps_bigin or steps_end position
    if (step_pos <= steps_home) {
      Serial.print("step_pos: ");
      Serial.println(step_pos);
      step_pos = steps_home;

      // Set motor direction clockwise
      // digitalWrite(dirPin, HIGH);
      dir_up = true;
      Serial.print("dir_up");

//      delay(500);
      delay(50);

    } else if (steps_turn <= step_pos) {
      Serial.print("step_pos: ");
      Serial.println(step_pos);

      step_pos = steps_turn;

      // Rotate servo motor up (Raise racket)
      for (sv_pos = 90; sv_pos >= 0; sv_pos -= 1) {
        myservo.write(sv_pos); 
        delay(10);
      }

      delay(500);

      // Rotate servo motor down
      for (sv_pos = 0; sv_pos <= 90; sv_pos += 1) {
        myservo.write(sv_pos);              
        delay(10);
      }

      // Set motor direction counterclockwise
      // digitalWrite(dirPin, LOW);
      dir_up = false;
      Serial.println("dir_up: false");

      delay(500);
    }

    // step by STEPS(steps pre rev.)/100
    if (dir_up == true) {
      stepper.step(x);
      step_pos = step_pos + x;  
    } else {
      stepper.step(-x);
      step_pos = step_pos -x;             
    }

  // Switch OFF
  } else {
    back_to_home();
  }
  digitalWrite(LswitchUp, HIGH);
  digitalWrite(LswitchDown, HIGH); 
}