// Encoder init.
#define PPR 240
#define WHEEL_DIAMETER_CM 10.0
#define GPIO_A 25 //Encoder A
#define GPIO_B 26 //Encoder B

volatile long pulseCount = 0;
float speed = 0.0;
unsigned long lastTime = 0;

void IRAM_ATTR onPulse() {
  pulseCount++;
}

void setup() {

  Serial.begin(115200);

  pinMode(GPIO_A, INPUT_PULLDOWN);
  pinMode(GPIO_B, INPUT_PULLDOWN);

  attachInterrupt(digitalPinToInterrupt(GPIO_A), onPulse, RISING);
}

void loop() {

  unsigned long currentTime = millis();

  if (currentTime - lastTime >= 1000) {
    float wheelCircumference = WHEEL_DIAMETER_CM * 3.14159 / 100.0; // in meters
    speed = (pulseCount * wheelCircumference) / (PPR * (currentTime - lastTime) / 1000.0); // meters per minute
    pulseCount = 0;
    lastTime = currentTime;

    // Output ke Serial untuk debugging
    Serial.print("Speed: ");
    Serial.print(speed);
    Serial.println(" m/s");
  }
}