#include <Servo.h> 

int servoPin = 3;
unsigned long servoDelay = 6;
unsigned long now;
unsigned long offWaitTime = 5000;
unsigned long closedTime;
unsigned long servoMoved;
int currServoVar;
Servo myservo;
#define closedButtPin 9
#define ENC_A 14
#define ENC_B 15
#define relayPin 11
#define ENC_PORT PINC
int servoVar; 
int servoOpen = 136;
int servoClosed = 22;
int openVar;
int prevCounter;
int closedVar;
int prevClosedVar;
int servoVarAdj;//adjusted servo var
void setup(){
  //////////rotary encoder stuff
  pinMode(ENC_A, INPUT);
  digitalWrite(ENC_A, HIGH);
  pinMode(ENC_B, INPUT);
  digitalWrite(ENC_B, HIGH);
  Serial.begin (9600);  Serial.println("Start");
  ///////////END Rotary stuff
  
  //////Servo Stuff
  myservo.attach(servoPin);
  myservo.write(70);
  ////END Servo stuff
  
  pinMode(closedButtPin, INPUT);
  pinMode(relayPin, OUTPUT);
  digitalWrite(relayPin, HIGH);
  
}


void loop(){

   now = millis();
  //////rotary stuff
   static uint8_t counter = 0;      //this variable will be changed by encoder input
 int8_t tmpdata;
 /**/
  tmpdata = read_encoder();
  if( tmpdata ) {
  // Serial.print("Counter value: ");
  //  Serial.println(counter, DEC);
    counter += tmpdata;
  }
  
  ///END rotary stuff
  
 
     
  //////switch stuff
  closedVar = digitalRead(closedButtPin);
  ////////end switch stuff
  
  if (counter > 150){
    counter = 0;
  }
  if(closedVar == 1){
    counter = 0;
    if(prevClosedVar == 0){
      closedTime = now;
    }
    if(now - closedTime > offWaitTime){
                  Serial.println("balls");

      digitalWrite(relayPin, LOW);

    }
  }

  servoVar = map(counter, 0,38,servoClosed,servoOpen);
  
  if(servoVar >= 0 && servoVar < 140){
    servoVarAdj = servoVar;
  }
  if(currServoVar != servoVarAdj){
    if(now - servoMoved > servoDelay){
      servoMoved = now;
      if(currServoVar > servoVarAdj){
        currServoVar--;
      }else{
        if(currServoVar < servoVarAdj){
          currServoVar++;
        }
      }
      myservo.write(currServoVar);
    }
  }
  
  
  
/*
Serial.print("counter = "); Serial.print(counter); 
Serial.print(" currServoVar = "); Serial.println(currServoVar);

Serial.print(" servoVarAdj = "); Serial.print(servoVarAdj);
Serial.print(" openVar = "); Serial.print(openVar);
Serial.print(" closedVar = "); Serial.println(closedVar);
*/
  
prevCounter = counter;
prevClosedVar = closedVar;

}
  
  
  
  


/* returns change in encoder state (-1,0,1) */
int8_t read_encoder()
{
  static int8_t enc_states[] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0};
  static uint8_t old_AB = 0;
  /**/
  old_AB <<= 2;                   //remember previous state
  old_AB |= ( ENC_PORT & 0x03 );  //add current state
  return ( enc_states[( old_AB & 0x0f )]);
}