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

#define NUM_LEG 4
#define COXA  20
#define FEMUR 159.1
#define TIBIA -6.5

const unsigned int ServoCoxa[NUM_LEG]={113,97,84,113}; //zero offset servo 1,2,3,4
const unsigned int ServoFemur[NUM_LEG]={120,90,120,90}; //zero offset servo 1,2,3,4
const unsigned int ServoTibia[NUM_LEG]={90,90,90,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};  

//panjang lengan
#define A0  27.5
#define A1  55
#define A2  77.5

//gerak robot
#define AX  90  
#define AY  90
#define AZ  90
#define Z_Offset  -30

//time
#define DT  25
#define T   1500

//rad
#define RAD 57.3

//0 diam, 1 gerak
#define Kerja 1

//Setpoint PID
float Kp_roll = 1.0, Ki_roll = 0.1, Kd_roll = 0.05;
float Kp_pitch = 1.0, Ki_pitch = 0.1, Kd_pitch = 0.05; 
float error[2]; 
float deltaerror[2];

float integralRoll = 0, integralPitch = 0;
float prevErrorRoll = 0, prevErrorPitch = 0;
float derivativeRoll, derivativePitch;
float outputRoll, outputPitch;

// 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

unsigned long cur_millis,prev_millis;
unsigned int t;

float c4[3],c2[3],c3[3],c1[3];
float sudut4[3],sudut2[3],sudut3[3],sudut1[3];

float rateRoll, ratePitch, rateYaw;
float accX, accY, accZ;
float angleRoll, anglePitch;

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

void setup() {
  Serial.begin(115200);
  Wire.setClock(400000);
  Wire.begin();
  setupIMU();
  init_servo();
  delay(3000);
  t=0;
  prev_millis=0;
}

void loop() {
  mainutama(error, deltaerror);
  
}

void mainutama(float error, float deltaerror){
  gyroSignal();
  stabilPID();
  fuzzyError(error);
  deltaFuzzyError(deltaerror);
  // fuzzy_rule(error, deltaError);

  cur_millis= millis();
  if(cur_millis-prev_millis>= DT){
    t=(t+DT)%T;
    trajektori4(t, T, c4);
    inverskin(c4, sudut4);
    trajektori2(t, T, c2);
    inverskin(c2, sudut2);
    trajektori3(t, T, c3);
    inverskin(c3, sudut3);
    trajektori1(t, T, c1);
    inverskin(c1, sudut1);

//angleRoll (tibia), anglePitch (femur)
    int tc1=ServoCoxa[0]+sudut1[0]*RAD;
    int tc2=ServoCoxa[1]-sudut2[0]*RAD;
    int tc3=ServoCoxa[2]+sudut3[0]*RAD;
    int tc4=ServoCoxa[3]-sudut4[0]*RAD;

    int tf1=ServoFemur[0]+sudut1[1]*RAD-FEMUR+anglePitch;
    int tf2=ServoFemur[1]-sudut2[1]*RAD+FEMUR+anglePitch;
    int tf3=ServoFemur[2]+sudut3[1]*RAD-FEMUR+anglePitch;
    int tf4=ServoFemur[3]-sudut4[1]*RAD+FEMUR+anglePitch;

    int tt1=ServoTibia[0]+sudut1[2]*RAD+TIBIA+angleRoll;
    int tt2=ServoTibia[1]+sudut2[2]*RAD+angleRoll;
    int tt3=ServoTibia[2]+sudut3[2]*RAD+TIBIA+angleRoll;
    int tt4=ServoTibia[3]+sudut4[2]*RAD+angleRoll;

    Serial.print(t/10);
    Serial.print("; ");

    // 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(tc4);
    // Serial.print("; ");
    // Serial.print(tf4);
    // Serial.print("; ");
    // Serial.print(tt4);
    // Serial.print("; ");
    // Serial.print(tc2);
    // Serial.print("; ");
    // Serial.print(tf2);
    // Serial.print("; ");
    // Serial.print(tt2);
    // Serial.print("; ");
    // Serial.print(tc3);
    // Serial.print("; ");
    // Serial.print(tf3);
    // Serial.print("; ");
    // Serial.print(tt3);
    // Serial.print("; ");
    // Serial.print(tc1);
    // Serial.print("; ");
    // Serial.print(tf1);
    // Serial.print("; ");
    // Serial.print(tt1);
    // Serial.print("; ");
    Serial.print(angleRoll);
    Serial.print("; ");
    Serial.print(anglePitch);
    Serial.print("; ");
    Serial.print(outputRoll);
    Serial.print("; ");
    Serial.print(outputPitch);
    Serial.print("; ");
    Serial.print(error);
    Serial.print("; ");
    Serial.println(deltaerror);

  // 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();

  prev_millis= millis();
  delay(100);
  }
}

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 trajektori4  (int time, int period, float *coordinate4){
  if((time> 0*period/6) && (time< (1*period)/11)){
  coordinate4[0]=-A1*cos(1.5*2*2*2*PI*time/period)+15.67;//X0
  }else if ((time> 1*period/10) && (time< (2*period)/6)){
  coordinate4[0]=70;//X0    
  }else if ((time> 2*period/6) && (time< (3*period)/7)){
  coordinate4[0]=(A1*cos(1.5*2*2*2*PI*time/period)/2)+28;//X0    
  }
  coordinate4[1]=A1*sin(2*2*2*2*3/2*PI*time/period);//Y0
  if((time>= 0*period/6) && (time< (1*period)/10)){
  coordinate4[2]=A1*sin(2*2*2*2*3/2*PI*(time+170)/period);}//Z0
}

void trajektori2(int time, int period, float *coordinate2){
  if((time> 0*period/6) && (time< (1*period)/6)){
  coordinate2[0]=-29;
  }else if((time> 1*period/8) && (time< (2*period)/8)){
  coordinate2[0]=-A1*cos(1.5*2*2*2*PI*(time+30)/period)+15.67;//X0
  }else if ((time> 1*period/10) && (time< (2*period)/6)){
  coordinate2[0]=70;//X0    
  }else if ((time> 2*period/6) && (time< (3*period)/7)){
  coordinate2[0]=(A1*cos(1.5*2*2*2*PI*time/period)/2)+28;//X0    
  }
  coordinate2[1]=A1*sin(2*2*2*2*3/2*PI*time/period);//Y0
  if((time>= 1*period/6) && (time< (2*period)/7.5)){
  coordinate2[2]=A1*sin(2*2*2*2*3/2*PI*(time+170)/period);}//Z0
}

void trajektori3(int time, int period, float *coordinate3){
  if((time> 0*period/6) && (time< (2*period)/10)){
    coordinate3[0]=-29;
  }else if ((time> 2*period/6) && (time< (3*period)/7)){
  coordinate3[0]=(A1*cos(1.5*2*2*2*PI*(time-0)/period)/2)-102.2;//X0    
  }else if ((time> 4*period/8) && (time< (5*period)/9)){
    coordinate3[0]=-A1*cos(1.5*2*2*2*PI*(time+30)/period)-47.6;//X0 
  }
  coordinate3[1]=A1*sin(2*2*2*2*3/2*PI*time/period);//Y0
  if((time>= 3*period/6) && (time< (4.2*period)/7)){
  coordinate3[2]=A1*sin(2*2*2*2*3/2*PI*(time+150)/period);}//Z0
}

void trajektori1(int time, int period, float *coordinate1){
  if((time> 0*period/6) && (time< (3*period)/9)){
    coordinate1[0]=-29;
  }else if ((time> 2*period/6) && (time< (3*period)/7)){
  coordinate1[0]=(A1*cos(1.5*2*2*2*PI*time/period)/2)-102.2;//X0    
  }else if ((time> 6*period/9) && (time< (7*period)/9)){
  coordinate1[0]=-A1*cos(1.5*2*2*2*PI*(time-20)/period)-54;//X0 
  }
  coordinate1[1]=A1*sin(2*2*2*2*3/2*PI*time/period);//Y0
  if((time>= 4*period/6) && (time< (5.3*period)/7)){
  coordinate1[2]=A1*sin(2*2*2*2*3/2*PI*(time+150)/period);}//Z0
}

void inverskin(float *coordinate, float *sudut){
  sudut[0]= atan(coordinate[0]/AX);//sudut coxa
  float d= sqrt(pow(coordinate[2],2)+pow(AX,2));
  sudut[2]= asin((pow(d,2)-(pow(A1,2)+pow(A2,2)))/(2*A1*A2));//sudut tibia
  sudut[1]= atan(coordinate[2]/(AX))+acos((pow(A2,2)-(pow(A1,2)+pow(d,2)))/(2*A1*d));//sudut femur
}

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));
    anglePitch = (-atan2(accX, sqrt(accY * accY + accZ * accZ)) * (180.0 / PI));

    delay(100);
}

void stabilPID(){
  gyroSignal();
  float setpoint=0;

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

  integralRoll += errorRoll* DT / 1500.0;
  integralPitch += errorPitch* DT / 1500.0;  
  
  prevErrorRoll = errorRoll;
  prevErrorPitch = errorPitch;

  derivativeRoll = (errorRoll - prevErrorRoll)/ (DT / 1500.0);
  derivativePitch = (errorPitch - prevErrorPitch)/ (DT / 1500.0);

  outputRoll = Kp_roll * errorRoll + Ki_roll * integralRoll + Kd_roll * derivativeRoll;
  outputPitch = Kp_pitch * errorPitch + Ki_pitch * integralPitch + Kd_pitch * derivativePitch;

  error = errorRoll; 
  deltaerror=derivativeRoll;

}

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