#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]={85,90,85,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

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

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

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 interval = 100;
unsigned long prevMillis = 0;

//Setpoint PID
float setpoint=0;

float Kp_roll = 0.75, Ki_roll = 0.001, Kd_roll = 0.02;
float Kp_pitch = 0.75, Ki_pitch = 0.001, Kd_pitch = 0.02; 

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

void setup() {
  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(115200);

  init_servo();

  time = millis();

  delay(1000);
  t=0;
  prev_millis=0;
}

void loop() {
  mainutama();
}

void mainutama(){
  stabilPID();
  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);

    int tc1=ServoCoxa[0]+sudut1[0]*RAD;
    int tc2=ServoCoxa[1]-sudut2[0]*RAD;
    int tc3=(ServoCoxa[2]+sudut3[0]*RAD)+35;
    int tc4=ServoCoxa[3]-sudut4[0]*RAD;

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

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

    // 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(realroll);
    Serial.print("; ");
    Serial.print(realpitch);
    Serial.print("; ");
    Serial.print(outputRoll);
    Serial.print("; ");
    Serial.print(outputPitch);
    Serial.println("; ");

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

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 mpu(){
  timePrev = time;  // the previous time is stored before the actual time read
  time = millis();  // actual time read
  elapsedTime = (time - timePrev) / 1000;

  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 registres
  Acc_rawY=Wire.read()<<8|Wire.read();
  Acc_rawZ=Wire.read()<<8|Wire.read();

  /*---X---*/
  Acceleration_angle[0] = atan((Acc_rawY/16384.0)/sqrt(pow((Acc_rawX/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;
  /*---Y---*/
  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 adress
  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();

  /*---X---*/
  Gyro_angle[0] = Gyr_rawX/131.0; 
  /*---Y---*/
  Gyro_angle[1] = Gyr_rawY/131.0;

  /*---X axis angle---*/
  Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + (0.02*Acceleration_angle[0]);
  /*---Y axis angle---*/
  Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + (0.02*Acceleration_angle[1]);

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

}

void stabilPID(){
  mpu();

  float errorRoll = setpoint-realroll;
  float errorPitch = setpoint-realpitch;

  integralRoll += errorRoll;
  integralPitch += errorPitch;  
  
  derivativeRoll = (errorRoll - prevErrorRoll);
  derivativePitch = (errorPitch - prevErrorPitch);

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

  prevErrorRoll = errorRoll;
  prevErrorPitch = errorPitch;
}