#include <Servo.h>

// Definición de pines
const int servoPin = 9;           // Pin para el servomotor
const int encoderClk = 2;         // Pin CLK del codificador (interruptión)
const int encoderDt = 3;          // Pin DT del codificador (interruptión)
const int encoderSw = 4;          // Pin SW del codificador (botón)

// Crear objeto Servo
Servo myServo;

// Variables del codificador
volatile int encoderPos = 0;       // Posición del codificador
int lastButtonState = HIGH;        // Estado anterior del botón
unsigned long lastDebounceTime = 0;// Tiempo de la última lectura del botón
const unsigned long debounceDelay = 50; // Retardo para debounce

// Variables del movimiento del servomotor
bool movimiento = false;           // Estado de movimiento del servomotor
int direccion = 1;                 // Dirección del movimiento: 1 = 0→180, -1 = 180→0
int currentPos = 0;                // Posición actual del servomotor

// Control de velocidad
unsigned long lastMoveTime = 0;    // Última vez que se movió el servomotor
unsigned long moveInterval = 1000; // Intervalo inicial muy lento (1000 ms por paso)
const unsigned long minInterval = 10;   // Intervalo mínimo (máxima velocidad)
const unsigned long maxInterval = 1000; // Intervalo máximo (mínima velocidad)

void setup() {
  // Iniciar comunicación serial para depuración (opcional)
  Serial.begin(9600);

  // Iniciar servomotor en posición 0
  myServo.attach(servoPin);
  myServo.write(currentPos);

  // Configurar pines del codificador
  pinMode(encoderClk, INPUT_PULLUP);
  pinMode(encoderDt, INPUT_PULLUP);
  pinMode(encoderSw, INPUT_PULLUP);

  // Adjuntar interrupciones para el codificador
  attachInterrupt(digitalPinToInterrupt(encoderClk), handleEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderDt), handleEncoder, CHANGE);
}

void loop() {
  // Leer el botón del codificador con debounce
  int reading = digitalRead(encoderSw);
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading == LOW && lastButtonState == HIGH) { // Botón presionado
      movimiento = !movimiento; // Alternar estado de movimiento
      if (movimiento) {
        // Iniciar movimiento
        direccion = 1; // Empezar hacia 180
        // Opcional: Asegurar que el servomotor comience desde 0 o 180
        if (currentPos != 0 && currentPos != 180) {
          myServo.write(0);
          currentPos = 0;
        }
      }
    }
  }
  lastButtonState = reading;

  // Mover el servomotor si está en movimiento
  if (movimiento) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastMoveTime >= moveInterval) {
      lastMoveTime = currentMillis;
      // Mover un paso de 1 grado
      if (direccion == 1) {
        if (currentPos < 180) {
          currentPos++;
        } else {
          direccion = -1; // Cambiar dirección
          currentPos--;
        }
      } else {
        if (currentPos > 0) {
          currentPos--;
        } else {
          direccion = 1; // Cambiar dirección
          currentPos++;
        }
      }
      myServo.write(currentPos);
      // Serial.print("Posición: ");
      // Serial.println(currentPos);
    }
  }

  // Ajustar la velocidad según la rotación del codificador
  if (encoderPos != 0) {
    moveInterval -= encoderPos * 10; // Ajuste de velocidad: cada 'detente' cambia el intervalo
    // Limitar el intervalo dentro de los valores permitidos
    if (moveInterval < minInterval) moveInterval = minInterval;
    if (moveInterval > maxInterval) moveInterval = maxInterval;
    encoderPos = 0; // Resetear contador
    // Serial.print("Intervalo de movimiento: ");
    // Serial.println(moveInterval);
  }
}

// Función para manejar las interrupciones del codificador
void handleEncoder() {
  int clkState = digitalRead(encoderClk);
  int dtState = digitalRead(encoderDt);

  // Determinar la dirección de giro
  if (clkState == HIGH) {
    if (dtState == LOW) {
      encoderPos++;
    } else {
      encoderPos--;
    }
  } else {
    if (dtState == HIGH) {
      encoderPos++;
    } else {
      encoderPos--;
    }
  }
}