#include <ESP32Encoder.h>

ESP32Encoder encoder;
ESP32Encoder encoder2;

hw_timer_t *My_timer = NULL;
volatile int64_t delta_omega_left = 0;
volatile int64_t delta_omega_right = 0;
volatile int64_t omega_left = 0;
volatile int64_t omega_right = 0;
int64_t new_omega_right, new_omega_left;

void IRAM_ATTR onTimer(){
new_omega_left = encoder.getCount();
delta_omega_left = new_omega_left - omega_left;
new_omega_right = encoder2.getCount();
delta_omega_right = new_omega_right - omega_right;
omega_left = new_omega_left;
omega_right = new_omega_right;
}

void setup(){
	
	Serial.begin(115200);

  My_timer = timerBegin(0, 80, true);
  timerAttachInterrupt(My_timer, &onTimer, true);
  timerAlarmWrite(My_timer, 100000, true);  //Millle number is the sample time in uS.
  timerAlarmEnable(My_timer); //Just Enable

	// Enable the weak pull up resistors
	ESP32Encoder::useInternalWeakPullResistors=UP;

	// use pin D5 and D22 for the first encoder
	encoder.attachHalfQuad(D5, D2);
	// use pin D3 and D4 for the second encoder (Order matters)
	encoder2.attachHalfQuad(D3, D4); 

	// clear the encoder's raw count and set the tracked count to zero
	encoder2.clearCount();
	Serial.println("Encoder Start = " + String((int32_t)encoder.getCount()));
}

void loop(){
	// Loop and read the count
	Serial.println("Encoder count = " + String((int32_t)encoder.getCount()) + " " + String((int32_t)encoder2.getCount()));
  Serial.println("Speed " + String((int32_t)delta_omega_left) + " " + String((int32_t)delta_omega_right));
  //Serial.println("Speed_right " + String((int32_t)delta_omega_right));
	delay(100);
}