#include <Servo.h> 

int servoPin = 3;
unsigned long servoDelay = 15;
unsigned long now;
unsigned long servoMoved;
int currServoVar;
Servo myservo;
#define openButtPin 10
#define closedButtPin 9
#define ENC_A 14
#define ENC_B 15
#define ENC_PORT PINC
int servoVar; 
int servoOpen = 138;
int servoClosed = 0;
int openVar;
int prevCounter;
int closedVar;
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 (115200);
  //Serial.println("Start");
  ///////////END Rotary stuff
  
  //////Servo Stuff
  myservo.attach(servoPin);
  myservo.write(0);
  ////END Servo stuff
}


void loop(){

  
  //////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
  
  
  //if counter is changing leave it alone to do its shit
  if(counter == prevCounter){
      now = millis();
  //////switch stuff
  closedVar = digitalRead(closedButtPin);
  openVar = digitalRead(openButtPin);
  ////////end switch stuff
  
  if(closedVar == 1){
    counter = 255;
  }
  if(openVar == 1){
    counter = 220;
  }
  servoVar = map(counter, 255,220,0,138);
  
  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(" servoVar = "); Serial.print(servoVar);
Serial.print(" servoVarAdj = "); Serial.print(servoVarAdj);
Serial.print(" openVar = "); Serial.print(openVar);
Serial.print(" closedVar = "); Serial.println(closedVar);
*/
  }
prevCounter = counter;
}
  
  
  
  


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