// Define fuzzy sets for error and change in error
#define LOW_ERROR 0
#define MEDIUM_ERROR 1
#define HIGH_ERROR 2

#define NEGATIVE 0
#define ZERO 1
#define POSITIVE 2

// Motor control pins
const int motorPin1 = 18;
const int motorPin2 = 19;

// Encoder input pins
const int encoderPin1 = 34;
const int encoderPin2 = 35;

// Variables for encoder and control
volatile int encoderPos = 0;
int encoderPosLast = 0;
double speedSetpoint = 200;
double speedMeasured = 0;

void setup() {
  pinMode(motorPin1, OUTPUT);
  pinMode(motorPin2, OUTPUT);
  pinMode(encoderPin1, INPUT);
  pinMode(encoderPin2, INPUT);

  attachInterrupt(digitalPinToInterrupt(encoderPin1), readEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(encoderPin2), readEncoder, CHANGE);

  Serial.begin(115200);
}

void loop() {
  speedMeasured = readRPM();
  int error = speedSetpoint - speedMeasured;
  int dError = error - (speedSetpoint - encoderPosLast);
  int output = fuzzyLogicControl(error, dError);

  // Control motor speed
  analogWrite(motorPin1, output);
  analogWrite(motorPin2, 0);

  Serial.print("RPM: ");
  Serial.println(speedMeasured);
  delay(100);
}

void readEncoder() {
  int state1 = digitalRead(encoderPin1);
  int state2 = digitalRead(encoderPin2);
  encoderPos += (state1 == state2) ? 1 : -1;
}

double readRPM() {
  int deltaPos = encoderPos - encoderPosLast;
  encoderPosLast = encoderPos;
  return (deltaPos / 20.0) * 60.0;  // Convert to RPM
}

int fuzzyLogicControl(int error, int dError) {
  int output = 0;

  // Fuzzy rules implementation
  if (error == LOW_ERROR && dError == ZERO) {
    output = 150; // Moderate speed
  } else if (error == HIGH_ERROR && dError == POSITIVE) {
    output = 255; // Full speed
  } else if (error == LOW_ERROR && dError == NEGATIVE) {
    output = 100; // Low speed
  }
  // Add more rules as necessary

  return output;
}