#include <Servo.h>
Servo servo1;

int homeValue;
int buttonState;
int encoderPos=97; 
int lastValue;  
int currentValue;
static unsigned long past;
static unsigned long past2;
static unsigned long past3;
int homeStatus;

void setup() {
  pinMode(2, OUTPUT);
  pinMode(3, INPUT);
  pinMode(5, INPUT_PULLUP);
  //pinMode(5, OUTPUT);
  pinMode(7, OUTPUT);
  //Value = digitalRead(3);
  pinMode(8, INPUT_PULLUP);
  pinMode(9, OUTPUT);
  pinMode(10, INPUT_PULLUP);

  // set PIN and range in us. Standard servo range is 500 - 2500 us
  servo1.attach(6, 500, 2500);

  Serial.begin(9600);
  Serial.println("BEGIN");
  Serial.println();
}

void loop() {
//Serial.println(encoderPos);
currentValue = digitalRead(3);
   if (currentValue != lastValue){
     //Clockwise
     if (digitalRead(4) != currentValue){  
       encoderPos ++;
       lastValue = currentValue;
       if (encoderPos>180) {
       encoderPos=180;
       digitalWrite(7, HIGH); //LimitLed ON
       past=millis();
       }
       Serial.println ("Clockwise");
       Serial.println(encoderPos);

       servo1.write(encoderPos);
     }

     //Counterclockwise
     else {
       encoderPos--;
       lastValue = currentValue;
       if (encoderPos<0) {
       encoderPos=0;
       digitalWrite(7, HIGH); //LimitLed ON
       past=millis();
       }
       Serial.println("Counterclockwise");
       Serial.println(encoderPos);

       servo1.write(encoderPos);
       }
   }

  //LimitLed OFF
  if (millis()-past>5000) {
  digitalWrite(7, LOW);
  }

  //==================== Home sequence start ====================

  //Setting homeStatus variable 0 or 1 by pressing encoder button - start
  if (digitalRead(5)==LOW) {
    homeStatus = 1;
    digitalWrite(2, HIGH);
    past3 = millis();
    }
    else {
    if (encoderPos == 90) {
    homeStatus = 0;
    digitalWrite(2, LOW);
    }
  }
  //Setting homeStatus 0 or 1 by pressing encoder button - end

  //Blinking Home LED when homing - start
  if (homeStatus == 1) {
    if (millis() - past3 > 500) {
    digitalWrite(2, !digitalRead(2));
    past3 = millis();
    }
  }
  //Blinking Home LED when homing - end

  if (homeStatus == 1) {
    if (encoderPos>90) {
      homeValue = encoderPos - 90;
      if (encoderPos!=90){
      
       if (millis() - past2 > 150) {
       encoderPos--;
       servo1.write(encoderPos);
       past2=millis();
       }
      }
    }

    else {
      homeValue = encoderPos - 90;
      if (encoderPos!=90){

       if (millis() - past2 > 150) {
       encoderPos++;
       servo1.write(encoderPos);
       past2=millis();
       }
      }
    }
  }
  //==================== Home sequence end ====================

  //==================== Home sequence interruption start ====================
  if (digitalRead(10) == LOW) {
  homeStatus = 0;
  digitalWrite(2, LOW);
  }
  //==================== Home sequence interruption end ====================

  //========== Push button responsiveness test start ==========
  if (digitalRead(8) == LOW) {
   digitalWrite(9, HIGH);
  }
  else {
   digitalWrite(9, LOW);
  }
  //========== Push button responsiveness test end ==========

}