#define encoderPinB         4
#define encoderPinA         2
#define encoderPressPin     5
#define MIN_ANGLE           1
#define MAX_ANGLE         179
#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     10

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

//#include <servo.h>
//Servo servo;

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 );
  enc_switch_old = digitalRead(encoderPressPin);
}

void loop() {
  // READ SWITCH AND CHANGE SERVO SETPOINT POINTER
  enc_switch = digitalRead(encoderPressPin);
  if (((enc_switch_old == 1) && enc_switch == 0)) enc_pressed_time = millis(); // 1->0 transition
  if ((enc_switch_old == 0) && (enc_switch == 1)) { // 0->1 transition
    if ((millis() - enc_pressed_time) > PRESS_TIME_LONG) { // long press
      pointer = 2; // servo to setpoint[2] = mid angle
      Serial.print("Long press: SETPOINT = ");
      Serial.println(setpoint[pointer]);
    }
    else if ((millis() - enc_pressed_time) > PRESS_TIME_SHORT) { // short press
      pointer = (pointer + 1) % 2;
      Serial.print("Short press: SETPOINT = ");
      Serial.println(setpoint[pointer]);
    }
    else Serial.println("SW BOUNCED");
  }
  enc_switch_old = enc_switch;

  // READ ROTARY ENCODER TO MODIFY SERVO SETPOINT
  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;
    }
    else if ((millis() - enc_rotated_time) > ROTARY_FAST) {
      increment = INCREMENT_FAST;
    }
    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]);
  }
  enc_clk_old = enc_clk;


}