#include <Servo.h>

#define ENCODER_CLK 2
#define ENCODER_DT  3
#define SERVO_PIN 5

Servo myservo;

volatile int servoPosition = 90; // Start at the middle position
int lockInPositions[5] = {35, 70, 105, 140, 175}; // Lock-in positions
int hysteresis = 3;
int maxServoPosition = 180;
int springZone = 18;
int springPosition = maxServoPosition - springZone;

void setup() {
  Serial.begin(115200);
  myservo.attach(SERVO_PIN);

  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);
}

void loop() {
  // Apply lock-in positions logic
/*   for (int i = 0; i < 5; i++) {
    if (abs(servoPosition - lockInPositions[i]) < hysteresis) {
      servoPosition = lockInPositions[i];
      break;
    }
  }
*/
  // Apply spring effect logic
  if (servoPosition >= springPosition) {
    servoPosition = maxServoPosition - springZone;
  } 

  myservo.write(servoPosition);
  delay(15);
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
        servoPosition = servoPosition + 18;
      } else {
        servoPosition = servoPosition - 18;

  }

  // Constrain servo position within 0 to 180 degrees
  servoPosition = constrain(servoPosition, 0, maxServoPosition);
}