// ------------------------- Definición de pines y variables -------------------------
const int A = 2;  // Entrada de la señal A del encoder
const int B = 3;  // Entrada de la señal B del encoder
const int Rodillo = 5;  // Sensor del rodillo (limit switch)
const int P1 = 7;  // Botón para resetear todo
const int P2 = 8;  // Botón para activar/desactivar el encoder

volatile int n = 0;  // Variable para almacenar el conteo del encoder
volatile byte ant = 0;  // Estado anterior del encoder
volatile byte act = 0;  // Estado actual del encoder
volatile bool encoderActivo = false;  // Variable para controlar si el encoder está activo
volatile bool rodilloPresionado = false;  // Indica si el rodillo ha sido presionado

unsigned long lastTime = 0;  // Almacena el tiempo anterior para control del muestreo
unsigned long sampleTime = 100;  // Intervalo de muestreo en milisegundos

double P = 0;  // Variable para almacenar la posición en grados
double R = 1438;  // Resolución del encoder (cantidad de pasos por revolución)

unsigned long startTime = 0;  // Tiempo de inicio de la oscilación
unsigned long oscilationDuration = 0;  // Duración total de la oscilación
bool timerActive = false;  // Indica si el temporizador está activo

double lastP = 0;  // Posición anterior para detectar estabilización
unsigned long stableTime = 0;  // Tiempo desde que el péndulo está estable
const unsigned long stabilityThreshold = 1000;  // 1 segundo para detectar estabilización
const double positionTolerance = 0.5;  // Tolerancia de variación en grados para determinar estabilidad

// ------------------------- Función de interrupción para el encoder -------------------------
void encoder() {
  if (encoderActivo) {
    ant = act;

    if (digitalRead(A)) 
      bitSet(act, 1); 
    else 
      bitClear(act, 1);
    
    if (digitalRead(B)) 
      bitSet(act, 0); 
    else 
      bitClear(act, 0);
    
    if (ant == 2 && act == 0) n++;
    if (ant == 0 && act == 1) n++;
    if (ant == 3 && act == 2) n++;
    if (ant == 1 && act == 3) n++;
    if (ant == 1 && act == 0) n--;
    if (ant == 3 && act == 1) n--;
    if (ant == 0 && act == 2) n--;
    if (ant == 2 && act == 3) n--;
    
    P = (360.0 * n) / R;

    unsigned long currentTime = millis();
    if (currentTime - lastTime >= sampleTime) {
      // Mostrar grados en tiempo real
      Serial.print("Grados: ");
      Serial.println(P);
      
      // Si el temporizador está activo, también muestra el tiempo de oscilación
      if (timerActive) {
        Serial.print(" | Tiempo de oscilación: ");
        Serial.print((currentTime - startTime) / 1000.0);  // Tiempo en segundos
        Serial.println(" segundos");
      }

      lastP = P;  // Actualiza la última posición
      lastTime = currentTime;  // Actualiza el tiempo anterior
    }
  } else {
    ant = 0;
    act = 0;
  }
}

// ------------------------- Función para el botón de reset -------------------------
void resetSystem() {
  Serial.println("Botón de reset presionado");

  // Reinicia solo los grados, pero mantiene activo el encoder
  n = 0;   // Reinicia el conteo del encoder
  P = 0;   // Reinicia la posición en grados
  lastP = 0;   // Reinicia la posición anterior
  
  // Detiene el temporizador y no muestra el tiempo
  timerActive = false;
  oscilationDuration = 0;
  stableTime = 0;  // Reinicia el tiempo estable
  
  // Imprime que los grados están en 0
  Serial.println("Grados: 0"); 

  // El encoder permanece activo para continuar la medición
  encoderActivo = true;
  rodilloPresionado = false;  // Restablece el estado del rodillo si es necesario
}

// ------------------------- Configuración inicial (setup) -------------------------
void setup() {
  Serial.begin(9600);
  
  pinMode(A, INPUT);
  pinMode(B, INPUT);
  pinMode(Rodillo, INPUT_PULLUP);
  pinMode(P1, INPUT_PULLUP);
  pinMode(P2, INPUT_PULLUP);
  
  attachInterrupt(digitalPinToInterrupt(A), encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(B), encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(P1), resetSystem, FALLING);
  
  Serial.println("Activa el Encoder para empezar ...");
}

// ------------------------- Bucle principal (loop) -------------------------
void loop() {
  // Control del encoder activado/desactivado con el botón P2
  if (digitalRead(P2) == LOW) {
    encoderActivo = !encoderActivo;
    if (encoderActivo) {
      Serial.println("Encoder activado.");
    } else {
      Serial.println("Encoder desactivado.");
    }
    delay(200);
  }

  // Control del rodillo
  if (digitalRead(Rodillo) == LOW) {
    if (!rodilloPresionado) {
      rodilloPresionado = true;
      Serial.println("Rodillo presionado. Comienza la oscilación y la medición del tiempo.");
      startTime = millis();  // Inicia el temporizador para la medición del tiempo
      timerActive = true;    // Activa el temporizador
    }
  }

  // Control del botón de reset
  if (digitalRead(P1) == LOW) {
    resetSystem();  // Llama a la función reset modificada
    delay(200);
  }
}