#include <Servo.h>

#define SERVO_PIN 11 // ESP32 pin GIOP26 connected to servo motor

// encoder pins
#define ENCODER_CLK 2
#define ENCODER_DT  3
#define ENCODER_SW 4
// max enconder steps for one rotation
#define ROTARY_MAX 20
int counter = 0;

//create Servo instance
Servo servoMotor;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println("Hello, I'm the servo king!");
  servoMotor.attach(SERVO_PIN);  // attaches the servo on ESP32 pin

    // Initialize encoder pins
  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);
  // attach Interupt for the detection of encoder changes
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
}

// method to read the encoder (called by interrupt)
void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
    counter++; // Clockwise
  }
  if (dtValue == LOW) {
    counter--; // Counterclockwise
  }
}

// Get the counter value, disabling interrupts.
// This make sure readEncoder() doesn't change the value
// while we're reading it.
int getCounter() {
  int result;
  noInterrupts();
  result = counter;
  interrupts();
  return result;
}

// resets the counter if the decoder is pressed
void resetCounter() {
  noInterrupts();
  counter = 0;
  interrupts();
}


void loop() {
  //calculate the postitiov of the servo based on the decoder posotion
  int pos = (180/ROTARY_MAX)*getCounter();
  
  //move the servo if the angle is betweet 0 and 180°
  // else turn on the onboard led
  if (pos<=180 && pos>=0) {
    digitalWrite(LED_BUILTIN, LOW);
    servoMotor.write(pos);
  } else {
    digitalWrite(LED_BUILTIN, HIGH);
  }

  //check if the decoder was pressed to reset the position
  if (digitalRead(ENCODER_SW) == LOW) {
    resetCounter();
  }

}