float error[7];
float derror[7];

void fuzifikkasi_error(float inp) {
  if (inp < -15) {
    error[0] = 1;
  } else if (inp < -10) {
    error[0] = (-10 - inp) / 5.0;
    error[1] = (inp + 15) / 5.0;
  } else if (inp < -5) {
    error[1] = (-5 - inp) / 5.0;
    error[2] = (inp + 10) / 5.0;
  } else if (inp < 0) {
    error[2] = -inp / 5.0;
    error[3] = (inp + 5) / 5.0;
  } else if (inp < 5) {
    error[3] = (5 - inp) / 5.0;
    error[4] = inp / 5.0;
  } else if (inp < 10) {
    error[4] = (10 - inp) / 5.0;
    error[5] = (inp - 5) / 5.0;
  } else if (inp < 15) {
    error[5] = (15 - inp) / 5.0;
    error[6] = (inp - 10) / 5.0;
  } else {
    error[6] = 1;
  }

  Serial.println("Hasil Fuzzifikasi Error:");
  for (int i = 0; i < 7; i++) {
    Serial.print("error[");
    Serial.print(i);
    Serial.print("]: ");
    Serial.println(error[i]);
  }
}

void fuzifikkasi_derror(float inp) {
  if (inp < -15) {
    derror[0] = 1;
  } else if (inp < -10) {
    derror[0] = (-10 - inp) / 5.0;
    derror[1] = (inp + 15) / 5.0;
  } else if (inp < -5) {
    derror[1] = (-5 - inp) / 5.0;
    derror[2] = (inp + 10) / 5.0;
  } else if (inp < 0) {
    derror[2] = -inp / 5.0;
    derror[3] = (inp + 5) / 5.0;
  } else if (inp < 5) {
    derror[3] = (5 - inp) / 5.0;
    derror[4] = inp / 5.0;
  } else if (inp < 10) {
    derror[4] = (10 - inp) / 5.0;
    derror[5] = (inp - 5) / 5.0;
  } else if (inp < 15) {
    derror[5] = (15 - inp) / 5.0;
    derror[6] = (inp - 10) / 5.0;
  } else {
    derror[6] = 1;
  }

  Serial.println("Hasil Fuzzifikasi Derror:");
  for (int i = 0; i < 7; i++) {
    Serial.print("derror[");
    Serial.print(i);
    Serial.print("]: ");
    Serial.println(derror[i]);
  }
}

int rule[7][7] = {
  {0, 0, 1, 1, 2, 2, 3},
  {0, 1, 1, 2, 2, 3, 4},
  {1, 1, 2, 2, 3, 4, 4},
  {1, 2, 2, 3, 4, 4, 5},
  {1, 2, 3, 4, 4, 5, 5},
  {2, 3, 4, 4, 5, 5, 6},
  {3, 4, 4, 5, 5, 6, 6}
};

float u[7];
float kp_values[7] = {1, 2, 3, 4, 5, 6, 7}; 
float ki_values[7] = {0.001, 0.002, 0.003, 0.004, 0.005, 0.006, 0.007}; 
float kd_values[7] = {0.01, 0.02, 0.03, 0.04, 0.05, 0.06, 0.07};
float kp, ki, kd;

int i, j, k;

void inferensi() {
  for (i = 0; i < 7; i++) {
    for (j = 0; j < 7; j++) {
      k = rule[i][j];
      u[k] = max(u[k], min(error[j], derror[i]));
    }
  }

  Serial.println("Hasil Inferensi (u):");
  for (int i = 0; i < 7; i++) {
    Serial.print("u[");
    Serial.print(i);
    Serial.print("]: ");
    Serial.println(u[i]);
  }
}

void defuzifikasi_kp() {
  float pemb = 0, peny = 0;
  for (int i = 0; i < 7; i++) {
    pemb += kp_values[i] * u[i];
    peny += u[i];
  }
  kp = (peny != 0) ? pemb / peny : 0;

  Serial.print("Output Kp: ");
  Serial.println(kp);
}

void defuzifikasi_ki() {
  float pemb = 0, peny = 0;
  for (int i = 0; i < 7; i++) {
    pemb += ki_values[i] * u[i];
    peny += u[i];
  }
  ki = (peny != 0) ? pemb / peny : 0;

  Serial.print("Output Ki: ");
  Serial.println(ki);
}

void defuzifikasi_kd() {
  float pemb = 0, peny = 0;
  for (int i = 0; i < 7; i++) {
    pemb += kd_values[i] * u[i];
    peny += u[i];
  }
  kd = (peny != 0) ? pemb / peny : 0;

  Serial.print("Output Kd: ");
  Serial.println(kd);
}

void setup() {
  Serial.begin(9600);
  Serial.println("Nilai Fuzzy untuk PID");

  float inp_error = -17.0;   
  float inp_derror = -9.0;   

  fuzifikkasi_error(inp_error);
  fuzifikkasi_derror(inp_derror);
  inferensi();
  
  // Defuzzifikasi untuk Kp, Ki, Kd
  defuzifikasi_kp();
  defuzifikasi_ki();
  defuzifikasi_kd();
}

void loop() {

}