#include <Arduino.h>
#include "math.h"
#include <Wire.h>

int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;

float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2], realroll, realpitch;

float elapsedTime, time, timePrev;
float rad_to_deg = 180 / 3.141592654;

unsigned long cur_millis, prev_millis;
unsigned int t;

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;
  }

  // Tampilkan hasil fuzzifikasi error
  // 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;
  }

  // Tampilkan hasil fuzzifikasi derror
  // 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 uk;
float u[7];

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

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

float c[7] = {-15, -10, -5, 0, 5, 10, 15};

void defuzifikasi() {
  // using sugeno
  float pemb = 0, peny = 0;
  for (int i = 0; i < 7; i++) {
    pemb += c[i] * u[i];
    peny += u[i];
  }

  uk = (peny != 0) ? pemb / peny : 0;

  // Tampilkan hasil defuzzifikasi
  Serial.print("Output Defuzzifikasi (uk): ");
  Serial.println(uk);
}

void setup() {
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(1000000);
  t = 0;
  prev_millis = 0;
  time = millis();


}

void loop() {
  mpu();
  // Serial.println("Nilai Fuzzy");

  float inp_error = realRoll;   // Contoh nilai input untuk error
  float inp_derror = -3.2;   // Contoh nilai input untuk derror

  fuzifikkasi_error(inp_error);
  fuzifikkasi_derror(inp_derror);
  inferensi();
  defuzifikasi();
  delay(1000);
}

void mpu() {
  timePrev = time;  // the previous time is stored before the actual time read
  time = millis();  // actual time read
  elapsedTime = (time - timePrev) / 1000.0;  // Convert to seconds

  Wire.beginTransmission(0x68);
  Wire.write(0x3B); // Ask for the 0x3B register- correspond to AcX
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true); 
    
  Acc_rawX = Wire.read() << 8 | Wire.read(); // each value needs two registers
  Acc_rawY = Wire.read() << 8 | Wire.read();
  Acc_rawZ = Wire.read() << 8 | Wire.read();

  Acceleration_angle[0] = atan((Acc_rawY / 16384.0) / sqrt(pow((Acc_rawX / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;
  Acceleration_angle[1] = atan(-1 * (Acc_rawX / 16384.0) / sqrt(pow((Acc_rawY / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;

  Wire.beginTransmission(0x68);
  Wire.write(0x43); // Gyro data first address
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 4, true); // Just 4 registers
  Gyr_rawX = Wire.read() << 8 | Wire.read();
  Gyr_rawY = Wire.read() << 8 | Wire.read();

  Gyro_angle[0] = Gyr_rawX / 131.0; 
  Gyro_angle[1] = Gyr_rawY / 131.0;

  Total_angle[0] = 0.98 * (Total_angle[0] + Gyro_angle[0] * elapsedTime) + (0.02 * Acceleration_angle[0]);
  Total_angle[1] = 0.98 * (Total_angle[1] + Gyro_angle[1] * elapsedTime) + (0.02 * Acceleration_angle[1]);

  realroll = -(Total_angle[0] - 2);
  realpitch = Total_angle[1];

  Serial.print(realroll);
  Serial.print("; ");
  Serial.print(realpitch);
  Serial.println("; ");
}