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

#define NUM_LEG 4
#define COXA  20
#define FEMUR 90
#define TIBIA -45

const unsigned int ServoCoxa[NUM_LEG]={90,90,90,90}; //zero offset servo C,B,F,D,A,E
const unsigned int ServoFemur[NUM_LEG]={90,90,90,90};   //zero offset servo C,B,F,D,A,E
const unsigned int ServoTibia[NUM_LEG]={90,90,90,90};   //zero offset servo C,B,F,D,A,E 
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];

void setup() {
  Serial.begin(9600);
  t=0;
  prev_millis=0;
}

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

    Serial.print(t/10);
    Serial.print("; ");
    Serial.print(ServoCoxa[0]+sudut4[0]*RAD);
    Serial.print("; ");
    Serial.print(ServoCoxa[1]+sudut2[0]*RAD);
    Serial.print("; ");
    Serial.print(ServoCoxa[2]+sudut3[0]*RAD);
    Serial.print("; ");
    Serial.print(ServoCoxa[3]+sudut1[0]*RAD);
    Serial.print("; ");
    Serial.println(ServoFemur[1]+sudut4[1]*RAD);
    // Serial.print("; ");
    // Serial.print(ServoFemur[1]+sudut2[1]*RAD);
    // Serial.print("; ");
    // Serial.print(ServoFemur[1]+sudut3[1]*RAD);
    // Serial.print("; ");
    // Serial.println(ServoFemur[1]+sudut1[1]*RAD);

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

void trajektori4  (int time, int period, float *coordinate4){
  if((time> 0*period/6) && (time< (1*period)/10)){
  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*PI*time/period);//Y0
  coordinate4[2]=A1*sin(2*PI*time/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*PI*time/period);//Y0
  coordinate2[2]=A1*sin(2*PI*time/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*PI*time/period);//Y0
  coordinate3[2]=A1*sin(2*PI*time/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*PI*time/period);//Y0
  coordinate1[2]=A1*sin(2*PI*time/period);//Z0
}

void inverskin(float *coordinate, float *sudut){
  sudut[0]= atan(coordinate[0]/AX);//sudut coxa
  float d= sqrt(pow(coordinate[2],2)+pow(coordinate[0],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
}