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

// Definisi array untuk menyimpan nilai fuzzy untuk error dan delta error
float errorFuzzyValues[5]; // Index: 0-ne_b, 1-ne_k, 2-zr, 3-po_k, 4-po_b
float deltaErrorValues[5]; // Index: 0-ne_b1, 1-ne_k1, 2-zr1, 3-po_k1, 4-po_b1

// Definisi array untuk menyimpan nilai rule fuzzy
float ruleKpValues[4]; // Index: 0-kp-zr, 1-kp-pk, 2-kp-ps, 3-kp-pb
float ruleKiValues[3]; // Index: 0-ki-zr, 1-ki-pk, 2-ki-pb
float ruleKdValues[3]; // Index: 0-kd-zr, 1-kd-pk, 2-kd-pb

#define NUM_LEG 4

const unsigned int ServoCoxa[NUM_LEG] = {113, 97, 84, 113}; //zero offset servo 1,2,3,4
const unsigned int ServoFemur[NUM_LEG] = {80, 101, 84, 90}; //zero offset servo 1,2,3,4
const unsigned int ServoTibia[NUM_LEG] = {105, 70, 98, 90}; //zero offset servo 1,2,3,4
const unsigned int pinCoxa[NUM_LEG] = {13, 12, 11, 10};
const unsigned int pinTibia[NUM_LEG] = {5, 4, 3, 2};
const unsigned int pinFemur[NUM_LEG] = {9, 8, 7, 6};

float rateRoll, ratePitch, rateYaw;
float accX, accY, accZ;
float angleRoll, anglePitch;
float prevErrorRoll, prevErrorPitch;
float integralRoll, integralPitch;

Servo coxa[NUM_LEG], femur[NUM_LEG], tibia[NUM_LEG];

//Setpoint PID
float setpoint = 0.0;
float kp, ki, kd;

void fuzzyError(float error) {
  // Reset nilai
  for(int i = 0; i < 5; i++) 
    errorFuzzyValues[i] = 0; 
  
  // Perhitungan untuk error
  if (error <= -10) {
    errorFuzzyValues[0] = 1; // ne_b
  } 
  else if (error > -10 && error < -5) {
    errorFuzzyValues[0] = (-5 - error) / 5; // ne_b
    errorFuzzyValues[1] = (error + 10) / 5; // ne_k
  } 
  else if (error >= -5 && error <= 0) {
    errorFuzzyValues[1] = (0 - error) / 5; // ne_k
    errorFuzzyValues[2] = (error + 5) / 5; // zr
  } 
  else if (error > 0 && error < 5) {
    errorFuzzyValues[2] = (5 - error) / 5; // zr
    errorFuzzyValues[3] = (error - 0) / 5; // po_k
  } 
  else if (error >= 5 && error < 10) {
    errorFuzzyValues[3] = (10 - error) / 5; // po_k
    errorFuzzyValues[4] = (error - 5) / 5; // po_b
  } 
  else if (error >= 10) {
    errorFuzzyValues[4] = 1; // po_b
  }
}

void deltaFuzzyError(float deltaError) {
  // Reset nilai
  for(int i = 0; i < 5; i++) 
    deltaErrorValues[i] = 0;

  // Perhitungan untuk delta error
  if (deltaError <= -2) {
    deltaErrorValues[0] = 1; // ne_b1
  } 
  else if (deltaError > -2 && deltaError < -1) {
    deltaErrorValues[0] = (-1 - deltaError); // ne_b1
    deltaErrorValues[1] = (deltaError + 2); // ne_k1
  } 
  else if (deltaError >= -1 && deltaError <= 0) {
    deltaErrorValues[1] = (0 - deltaError); // ne_k1
    deltaErrorValues[2] = (deltaError + 1); // zr1
  } 
  else if (deltaError > 0 && deltaError < 1) {
    deltaErrorValues[2] = (1 - deltaError); // zr1
    deltaErrorValues[3] = (deltaError - 0); // po_k1
  } 
  else if (deltaError >= 1 && deltaError < 2) {
    deltaErrorValues[3] = (2 - deltaError); // po_k1
    deltaErrorValues[4] = (deltaError - 1); // po_b1
  } 
  else if (deltaError >= 2) {
    deltaErrorValues[4] = 1; // po_b1
  }
}

// Rule Fuzzy 
void fuzzy_rule(float error, float deltaError) {
  // Reset nilai
  for(int i = 0; i < 4; i++) 
    ruleKpValues[i] = 0;
  for(int i = 0; i < 3; i++) {
    ruleKiValues[i] = 0;
    ruleKdValues[i] = 0;
  }

  // Daftar aturan fuzzy
  const int rules[25][3] = {
    {0, 0, 1}, {1, 0, 1}, {2, 1, 0}, {2, 2, 0}, {3, 2, 0}, 
    {1, 0, 2}, {2, 1, 1}, {1, 1, 1}, {2, 1, 1}, {2, 2, 2}, 
    {1, 0, 2}, {2, 1, 1}, {0, 0, 0}, {2, 1, 1}, {2, 2, 2}, 
    {1, 0, 2}, {1, 1, 1}, {2, 1, 1}, {1, 1, 1}, {1, 2, 2}, 
    {0, 0, 2}, {1, 0, 2}, {2, 1, 2}, {3, 1, 1}, {1, 2, 1}
  };

  for (int i = 0; i < 25; i++) {
    if (errorFuzzyValues[i / 5] && deltaErrorValues[i % 5]) {
      ruleKpValues[rules[i][0]] = max(ruleKpValues[rules[i][0]], min(errorFuzzyValues[i / 5], deltaErrorValues[i % 5]));
      ruleKiValues[rules[i][1]] = max(ruleKiValues[rules[i][1]], min(errorFuzzyValues[i / 5], deltaErrorValues[i % 5]));
      ruleKdValues[rules[i][2]] = max(ruleKdValues[rules[i][2]], min(errorFuzzyValues[i / 5], deltaErrorValues[i % 5]));
    }
  }
  
  kp = ruleKpValues[0] * 0 + ruleKpValues[1] * 3.33 + ruleKpValues[2] * 6.66 + ruleKpValues[3] * 10;
  ki = ruleKiValues[0] * 0 + ruleKiValues[1] * 0.75 + ruleKiValues[2] * 1.5;
  kd = ruleKdValues[0] * 0 + ruleKdValues[1] * 2.5 + ruleKdValues[2] * 5;
}

void displayFuzzyValues(){
  // Menampilkan nilai fuzzy error
  Serial.print("Fuzzy Error: ");
  for (int i = 0; i < 5; i++) {
    Serial.print(errorFuzzyValues[i], 2); // Tampilkan dengan 2 digit desimal
    Serial.print(" ");
  }
  Serial.println();

  // Menampilkan nilai delta fuzzy error
  Serial.print("Delta Fuzzy Error: ");
  for (int i = 0; i < 5; i++) {
    Serial.print(deltaErrorValues[i], 2); // Tampilkan dengan 2 digit desimal
    Serial.print(" ");
  }
  Serial.println();

  Serial.print("Gain kp : ");
  for (int i = 0; i < 4; i++) {
    Serial.print(ruleKpValues[i], 2); // Tampilkan dengan 2 digit desimal
    Serial.print(" ");
  }
  Serial.println();

  Serial.print("Gain ki : ");
  for (int i = 0; i < 3; i++) {
    Serial.print(ruleKiValues[i], 2); // Tampilkan dengan 2 digit desimal
    Serial.print(" ");
  }
  Serial.println();

  Serial.print("Gain kd : ");
  for (int i = 0; i < 3; i++) {
    Serial.print(ruleKdValues[i], 2); // Tampilkan dengan 2 digit desimal
    Serial.print(" ");
  }
  Serial.println();
}

void setupIMU() {
    // Inisialisasi MPU6050
    Wire.beginTransmission(0x68);
    Wire.write(0x6B);
    Wire.write(0x00); // Membebaskan dari mode tidur
    Wire.endTransmission();

    // Pengaturan filter akselerometer
    Wire.beginTransmission(0x68);
    Wire.write(0x1C); 
    Wire.write(0x10); // Pengaturan sensivitas +/- 8g
    Wire.endTransmission();

    // Pengaturan filter giroskop
    Wire.beginTransmission(0x68);
    Wire.write(0x1B); 
    Wire.write(0x08); // Pengaturan sensitivitas +/- 500 deg/s
    Wire.endTransmission();
}

void init_servo() {  
  for (int i = 0; i < NUM_LEG; i++) {
    coxa[i].attach(pinCoxa[i]);
    femur[i].attach(pinFemur[i]);
    tibia[i].attach(pinTibia[i]);
  }
}

void gyroSignal() {
    // Baca data akselerometer
    Wire.beginTransmission(0x68);
    Wire.write(0x3B);
    Wire.endTransmission();
    Wire.requestFrom(0x68, 6);
    int16_t accXLSB = Wire.read() << 8 | Wire.read();
    int16_t accYLSB = Wire.read() << 8 | Wire.read();
    int16_t accZLSB = Wire.read() << 8 | Wire.read();

    // Baca data giroskop
    Wire.beginTransmission(0x68);
    Wire.write(0x43);
    Wire.endTransmission();
    Wire.requestFrom(0x68, 6);
    int16_t gyroX = Wire.read() << 8 | Wire.read();
    int16_t gyroY = Wire.read() << 8 | Wire.read();
    int16_t gyroZ = Wire.read() << 8 | Wire.read();

    // Hitung kecepatan sudut giroskop
    rateRoll = (float)gyroX / 65.5;
    ratePitch = (float)gyroY / 65.5;
    rateYaw = (float)gyroZ / 65.5;

    // Hitung akselerasi
    accX = (float)accXLSB / 4096.0;
    accY = (float)accYLSB / 4096.0;
    accZ = (float)accZLSB / 4096.0;

    // Hitung sudut menggunakan akselerometer
    angleRoll = (atan2(accY, sqrt(accX * accX + accZ * accZ)) * (180.0 / PI))-3.3;
    anglePitch = (-atan2(accX, sqrt(accY * accY + accZ * accZ)) * (180.0 / PI))+0.1;
    delay(200);
}

void setup() {
  Serial.begin(115200);
  Wire.setClock(400000);
  Wire.begin();
  setupIMU();
  init_servo();

  prevErrorRoll = 0.0;
  prevErrorPitch = 0.0;
  integralRoll = 0.0;
  integralPitch = 0.0;
}

void loop() {
    gyroSignal();

    float errorRoll = setpoint - angleRoll;
    float errorPitch = setpoint - anglePitch;

    integralRoll += errorRoll;
    integralPitch += errorPitch;

    float derivativeRoll = (errorRoll - prevErrorRoll);
    float derivativePitch = (errorPitch - prevErrorPitch);

    fuzzyError(errorRoll);
    deltaFuzzyError(derivativeRoll);
    fuzzy_rule(errorRoll, derivativeRoll);
    displayFuzzyValues();

    // Menggunakan nilai fuzzy untuk PID
    float pidRoll = kp * errorRoll + ki * integralRoll + kd * derivativeRoll;
    float pidPitch = kp * errorPitch + ki * integralPitch + kd * derivativePitch;

    prevErrorRoll = errorRoll;
    prevErrorPitch = errorPitch;

    int tc1 = ServoCoxa[0];
    int tc2 = ServoCoxa[1];
    int tc3 = ServoCoxa[2];
    int tc4 = ServoCoxa[3];

    int tf1 = ServoFemur[0];
    int tf2 = ServoFemur[1];
    int tf3 = ServoFemur[2];
    int tf4 = ServoFemur[3];

    int tt1 = ServoTibia[0];
    int tt2 = ServoTibia[1];
    int tt3 = ServoTibia[2];
    int tt4 = ServoTibia[3];  

    coxa[3].write(tc4);
    femur[3].write(tf4);
    tibia[3].write(tt4);
    
    coxa[1].write(tc2);
    femur[1].write(tf2);
    tibia[1].write(tt2);

    coxa[2].write(tc3);
    femur[2].write(tf3);
    tibia[2].write(tt3);

    coxa[0].write(tc1);
    femur[0].write(tf1);
    tibia[0].write(tt1);
        
    Serial.print("Roll: ");
    Serial.print(angleRoll);
    Serial.print(" Pitch: ");
    Serial.print(anglePitch);
    
    Serial.print(" pid Roll: ");
    Serial.print(pidRoll);
    Serial.print(" pid Pitch: ");
    Serial.println(pidPitch);
}