/************* DÉFINITION DES BROCHES *************/
#define ENCA 2
#define ENCB 3

#define PIN_IN1  8
#define PIN_IN2  9
#define PIN_ENA  10

/************* PARAMÈTRES MÉCANIQUES *************/
#define RATIO 9.6
#define CPR_MOTOR 11
#define CPR_TOTAL (CPR_MOTOR * RATIO)

/************* FILTRE *************/
#define ALPHA 0.85   // filtre passe-bas 1er ordre

/************* VARIABLES TEMPS *************/
unsigned long currT = 0;
unsigned long prevT = 0;
unsigned long t0 = 0;

float t = 0;          // temps absolu (s)
float deltaT = 0;     // période d’échantillonnage (s)

/************* ENCODEUR *************/
volatile long posi = 0;
long lastPosi = 0;

/************* VITESSE *************/
float rpm = 0;        // vitesse brute
float rpmFilt = 0;    // vitesse filtrée

/******************** SETUP ********************/
void setup() {

  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_ENA, OUTPUT);

  pinMode(ENCA, INPUT);
  pinMode(ENCB, INPUT);

  attachInterrupt(digitalPinToInterrupt(ENCA),
                  readEncoderA,
                  RISING);

  Serial.begin(9600);

  // En-tête CSV
  Serial.println("t,rpm,rpmFilt");

  // Référence temporelle
  t0 = micros();
  prevT = t0;

  // Sens fixe
  digitalWrite(PIN_IN1, HIGH);
  digitalWrite(PIN_IN2, LOW);

  // PWM fixe
  analogWrite(PIN_ENA, 150);
}

/******************** LOOP ********************/
void loop() {

  /************* TEMPS *************/
  currT = micros();
  deltaT = (currT - prevT) / 1.0e6;
  prevT = currT;

  if (deltaT <= 0) return;

  t = (currT - t0) / 1.0e6;

  /************* POSITION *************/
  long pos;
  noInterrupts();
  pos = posi;
  interrupts();

  long deltaP = pos - lastPosi;
  lastPosi = pos;

  /************* VITESSE BRUTE *************/
  rpm = (deltaP / deltaT) * (60.0 / CPR_TOTAL);

  /************* FILTRE PASSE-BAS *************/
  rpmFilt = ALPHA * rpmFilt + (1.0 - ALPHA) * rpm;

  /************* AFFICHAGE CSV *************/
  Serial.print(t, 3);        // temps avec 3 décimales
  Serial.print(",");
  Serial.print(rpm, 1);
  Serial.print(",");
  Serial.println(rpmFilt, 1);
}

/**************** INTERRUPTION ****************/
void readEncoderA() {

  int b = digitalRead(ENCB);

  if (b > 0) {
    posi++;
  } else {
    posi--;
  }
}