#include <PID_v1.h>  // Inclui a biblioteca PID_v1

#define POT_PIN 34        // Pino do ESP32 conectado ao potenciômetro (pino analógico)
#define MOTOR_PWM_PIN 13  // Pino do ESP32 conectado ao motor (controlado via PWM)
#define OPTICAL_SENSOR_PIN 12  // Pino do ESP32 conectado ao sensor óptico (digital)
#define BUTTON_PIN 14      // Pino do ESP32 conectado ao botão de controle (entrada digital)

double potValue = 0;          // Valor lido do potenciômetro (0 a 1023)
double targetRPM = 0;         // RPM alvo (definido pelo potenciômetro)
double currentRPM = 0;        // RPM atual (calculado a partir do sensor óptico)
double motorSpeed = 0;        // Saída do PID que define a velocidade do motor (0 a 255)

volatile int rotationCount = 0; // Contador de rotações
unsigned long lastTime = 0;
unsigned long interval = 1000; // Intervalo para contar as rotações (1 segundo)

// Definindo as variáveis do PID
double inputRPM, outputPWM, setpointRPM;

// Parâmetros do PID (ajuste fino desses valores será necessário)
double Kp = 2.0, Ki = 0.1, Kd = 1.0;

// Instanciando o controlador PID
PID myPID(&inputRPM, &outputPWM, &setpointRPM, Kp, Ki, Kd, DIRECT);

bool controlActive = false;  // Flag para ativar o controle PID

void IRAM_ATTR countRotation() {
  // Incrementa o contador de rotações toda vez que o sensor óptico detecta o marcador
  rotationCount++;
}

void setup() {
  // Inicia a comunicação serial para monitorar os valores
  Serial.begin(115200);

  // Configura os pinos
  pinMode(MOTOR_PWM_PIN, OUTPUT);  // Pino do motor como saída
  pinMode(POT_PIN, INPUT);         // Pino do potenciômetro como entrada
  pinMode(OPTICAL_SENSOR_PIN, INPUT_PULLUP);  // Pino do sensor óptico como entrada com pull-up
  pinMode(BUTTON_PIN, INPUT_PULLUP);  // Pino do botão de controle como entrada com pull-up

  // Ativa a interrupção para detectar o sinal do sensor óptico
  attachInterrupt(digitalPinToInterrupt(OPTICAL_SENSOR_PIN), countRotation, FALLING);

  // Inicializa o PID
  setpointRPM = 0;   // Inicializa a RPM desejada como 0
  inputRPM = 0;      // Inicializa a RPM atual como 0
  outputPWM = 0;     // Inicializa a saída PWM
  myPID.SetMode(AUTOMATIC); // Define o controlador PID para modo automático

  // Inicializa a flag de controle
  controlActive = false; // O controle começa desativado
}

void loop() {
  // Lê o valor do potenciômetro (0 a 4095)
  potValue = analogRead(POT_PIN);

  // Mapeia o valor do potenciômetro para o intervalo de RPM desejado (ex: 0 a 200 RPM)
  targetRPM = map(potValue, 0, 4095, 0, 200);

  // Verifica se o botão foi pressionado (pino com pull-up, então será LOW quando pressionado)
  if (digitalRead(BUTTON_PIN) == LOW) {
    controlActive = true;  // Ativa o controle do motor
    Serial.println("Controle ativado");
  }

  // Se o controle estiver ativado, faz o cálculo do controle PID
  if (controlActive) {
    // Verifica se é hora de calcular as rotações por segundo
    unsigned long currentTime = millis();
    if (currentTime - lastTime >= interval) {
      // Calcula as rotações por minuto (RPM) com base no contador de rotações
      currentRPM = (rotationCount * 60) / (interval / 1000); // RPM = (contagem de rotações * 60) / intervalo (em segundos)
      rotationCount = 0; // Reseta o contador de rotações

      // Atualiza o valor do PID
      inputRPM = currentRPM;    // Define a RPM atual como entrada do PID
      setpointRPM = targetRPM;   // Define a RPM alvo como setpoint do PID

      // Chama o PID para calcular a nova saída de PWM
      myPID.Compute();

      // A saída PWM deve ser entre 0 e 255
      motorSpeed = constrain(outputPWM, 0, 255); 

      // Ajusta o PWM para controlar a velocidade do motor
      analogWrite(MOTOR_PWM_PIN, motorSpeed);

      // Exibe as informações no monitor serial
      Serial.print("Potenciômetro: ");
      Serial.print(potValue);
      Serial.print(" | RPM Alvo: ");
      Serial.print(targetRPM);
      Serial.print(" | RPM Atual: ");
      Serial.print(currentRPM);
      Serial.print(" | PWM: ");
      Serial.println(motorSpeed);

      // Atualiza o tempo de contagem
      lastTime = currentTime;
    }
  } else {
    // Caso o controle não esteja ativado, o motor não é acionado
    analogWrite(MOTOR_PWM_PIN, 0);  // Desliga o motor
    Serial.println("Aguardando ativação do controle...");
  }

  // Atraso para estabilidade na leitura
  delay(100);
}