#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; 

const int steps_begin = 0;
const int steps_end = 3200;
const int steps_home = 100;
 
void lightLED(){
  if (sw_on){
    digitalWrite(ledWhite, LOW);
    digitalWrite(ledRed, HIGH);
  } else {
    digitalWrite(ledWhite, HIGH);
    digitalWrite(ledRed, LOW);
  }
}

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 != 0) {
    stepper.step(-step_pos);
    step_pos = 0;
  }
  delay(1000);
}

void edgeUp_to_home(){
  // step down to home
  stepper.step(steps_home - steps_end);
  dir_up = true;
  step_pos = 0;
}

void edgeDown_to_home(){
  // stepup to home
  stepper.step(steps_home);
  dir_up = true;
  step_pos = 0;
}

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);
    // Toggle direction to Down if Limit switch UP is hit
    if( (digitalRead(LswitchUp) == LOW) && (edgeUp == false) ) {
      Serial.println("edgeUp colision detected"); 
      break;
    }
  }
  edgeUp_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);
  }

  // Toggle direction to Down if Limit switch UP is hit
  if( (digitalRead(LswitchUp) == LOW) && (edgeUp == false) ) {
    Serial.println("edgeUp colision detected"); 
    edgeUp = true; 
    
//    edgeUp_to_home();
    dir_up = false;
    // delay(20);

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

  // Toggle direction to Up if Limit switch Down is hit
  if( (digitalRead(LswitchDown) == LOW) && (edgeDown == false) ) {
    Serial.println("edgeDown colision detected"); 
    edgeDown = true; 
    
//    edgeDown_to_home();
    dir_up = true;
    // delay(20);

  } 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_begin) {
      Serial.print("step_pos: ");
      Serial.println(step_pos);
      step_pos = steps_begin;

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

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

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

      step_pos = steps_end;

      // 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); 
}