#define encoderPinA         2
#define encoderPinB         4
#define encoderPressPin     5
#define MIN_ANGLE           0
#define MAX_ANGLE           80000
#define PRESS_TIME_SHORT    20 // [ms] short press
#define PRESS_TIME_LONG     500 // [ms] long  press
#define ROTARY_FAST         10 // [ms] rotary speed fast
#define ROTARY_SLOW         200 // [ms] rotary speed slow
#define INCREMENT_SLOW      1
#define INCREMENT_FAST      1000

unsigned long increment;
unsigned long pointer; // pointer to setpoint[pointer]
byte enc_clk;
byte enc_clk_old;
byte enc_switch;
byte enc_switch_old;
unsigned long angle = (MIN_ANGLE + MAX_ANGLE) / 2;
unsigned long setpoint[3] = {angle - 5, angle + 5, angle};
unsigned long time_for_servo, enc_pressed_time, enc_rotated_time;
byte fast_slow ;

unsigned long counter = 0;


void setup() {
  pinMode (encoderPinA, INPUT_PULLUP);
  pinMode (encoderPinB, INPUT_PULLUP);
  pinMode (encoderPressPin, INPUT_PULLUP);
  Serial.begin (9600);
  //Serial.print("SETPOINT = ");
  //Serial.println(setpoint[pointer]);
  enc_clk_old    = digitalRead(encoderPinA );


}

void loop() {

  enc_clk = digitalRead(encoderPinA );
  if ((enc_clk == 1) && (enc_clk_old == 0)) { // 0->1 transition
    if ((millis() - enc_rotated_time) > ROTARY_SLOW) {
      increment = INCREMENT_SLOW;
      counter = increment + counter;
      fast_slow = 0;
      Serial.println(counter );
    }
    else if ((millis() - enc_rotated_time) > ROTARY_FAST) {
      increment = INCREMENT_FAST;
      counter = increment + counter;
      fast_slow = 1;
      Serial.println(counter );
    }
    else {
      Serial.println("ROT BOUNCED");
      increment = 0;
    }
    enc_rotated_time = millis();
    if (digitalRead(encoderPinB ) == 1)
      setpoint[pointer] = setpoint[pointer] + increment;
    else
      setpoint[pointer] = setpoint[pointer] - increment;
    if (setpoint[pointer] > MAX_ANGLE) setpoint[pointer] = MAX_ANGLE;
    if (setpoint[pointer] < MIN_ANGLE) setpoint[pointer] = MIN_ANGLE;
    //Serial.println(setpoint[pointer]);
    //Serial.println(counter + increment);
  }
  enc_clk_old = enc_clk;


}