#include <Servo.h>

//L1 & L4: 
Servo L1_r;
Servo L1_k;
Servo L1_f;
//
Servo L4_r;
Servo L4_k;
Servo L4_f;

//L2 & L3: 
Servo L2_r;
Servo L2_k;
Servo L2_f;
//
Servo L3_r;
Servo L3_k;
Servo L3_f;

//base: 
int roll_a = 1267; 
int knee_a = 1422; 
int feet_a = 1623; 
//change: 
int roll_a_b = 1355; 
int knee_a_b = 1590; 
int feet_a_b = 1314; 

//L1 & L4:
//ROLL:
//L1:
int X = 0; 
int Y = 0; 
//L4:
int Z = 0; 
int W = 0; 

//KNEE:
//L1:
int T = 0; 
int R = 0; 
//L4:
int U = 0; 
int V = 0; 

//FEET:
//L1:
int P = 0; 
int O = 0; 
//L4:
int S = 0; 
int Q = 0; 

//L2 & L3: 
//ROLL:
//L2:
int N = 0; 
int L = 0; 
//L3:
int M = 0; 
int K = 0; 

//KNEE:
//L2: 
int J = 0; 
int I = 0; 
//L3:
int H = 0; 
int G = 0; 

//FEET:
//L2:
int F = 0; 
int E = 0; 
//L3:
int D = 0; 
int C = 0; 


//
float stepsPerMove = 40; 

int toglle = 48;

//L1 & L4 ROLL:
void IK_roll(double x, double y){
   double a = atan2(y, x) * (180 / 3.1415); 

   double l = sqrt(x * x + y * y); 

   double b = atan(x / y) * (180 / 3.1415); 

   double a1 = b; 

   L1_r.write(a1); 
}

void IK_roll_L4(double x, double y){
   double a = atan2(y, x) * (180 / 3.1415); 

   double l = sqrt(x * x + y * y); 

   double b = atan(x / y) * (180 / 3.1415); 

   double a1 = b; 

   L4_r.write(a1); 
}

//L2 & L3 ROLL:
void IK_roll_L2(double x, double y){
   double a = atan2(y, x) * (180 / 3.1415); 

   double l = sqrt(x * x + y * y); 

   double b = atan(x / y) * (180 / 3.1415); 

   double a1 = b; 

   L2_r.write(a1); 
}

void IK_roll_L3(double x, double y){
   double a = atan2(y, x) * (180 / 3.1415); 

   double l = sqrt(x * x + y * y); 

   double b = atan(x / y) * (180 / 3.1415); 

   double a1 = b; 

   L3_r.write(a1); 
}

//L1 & L4 KNEE:
void IK_knee(double t, double r){
   double a = atan2(r, t) * (180 / 3.1415); 

   double l = sqrt(t * t + r * r); 

   double b = atan(t / r) * (180 / 3.1415); 

   double a1 = b; 

   L1_k.write(a1); 
}

void IK_knee_L4(double t, double r){
   double a = atan2(r, t) * (180 / 3.1415); 

   double l = sqrt(t * t + r * r); 

   double b = atan(t / r) * (180 / 3.1415); 

   double a1 = b; 

   L4_k.write(a1); 
}

//L2 & L3 KNEE:
void IK_knee_L2(double t, double r){
   double a = atan2(r, t) * (180 / 3.1415); 

   double l = sqrt(t * t + r * r); 

   double b = atan(t / r) * (180 / 3.1415); 

   double a1 = b; 

   L2_k.write(a1); 
}

void IK_knee_L3(double t, double r){
   double a = atan2(r, t) * (180 / 3.1415); 

   double l = sqrt(t * t + r * r); 

   double b = atan(t / r) * (180 / 3.1415); 

   double a1 = b; 

   L3_k.write(a1); 
}

//L1 & L4 FEET:
void IK_feet(double p, double o){
   double a = atan2(o, p) * (180 / 3.1415); 

   double l = sqrt(p * p + o * o); 

   double b = atan(p / o) * (180 / 3.1415); 

   double a1 = b; 

   L1_f.write(a1); 
}

void IK_feet_L4(double p, double o){
   double a = atan2(o, p) * (180 / 3.1415); 

   double l = sqrt(p * p + o * o); 

   double b = atan(p / o) * (180 / 3.1415); 

   double a1 = b; 

   L4_f.write(a1); 
}

//L2 & L3 FEET:
void IK_feet_L2(double p, double o){
   double a = atan2(o, p) * (180 / 3.1415); 

   double l = sqrt(p * p + o * o); 

   double b = atan(p / o) * (180 / 3.1415); 

   double a1 = b; 

   L2_f.write(a1); 
}

void IK_feet_L3(double p, double o){
   double a = atan2(o, p) * (180 / 3.1415); 

   double l = sqrt(p * p + o * o); 

   double b = atan(p / o) * (180 / 3.1415); 

   double a1 = b; 

   L3_f.write(a1); 
}

//L1 & L4:
void IK_BySteps_roll(double x, double y, double z, double w){
  //L1:
  double xDelta = (x - X) / stepsPerMove;
  double yDelta = (y - Y) / stepsPerMove;
  //L4:
  double zDelta = (z - Z) / stepsPerMove;
  double wDelta = (w - W) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    X += xDelta; 
    Y += yDelta;
    IK_roll(X, Y); 
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    Z += zDelta; 
    W += wDelta;
    IK_roll_L4(Z, W); 
    delay(100 / stepsPerMove);   
  } 
}

void IK_BySteps_knee(double t, double r, double u, double v){
  //L1:
  double tDelta = (t - T) / stepsPerMove;
  double rDelta = (r - R) / stepsPerMove;
  //L4:
  double uDelta = (u - U) / stepsPerMove;
  double vDelta = (v - V) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    T += tDelta; 
    R += rDelta;
    IK_knee(T, R); 
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    U += uDelta; 
    V += vDelta;
    IK_knee_L4(U, V); 
    delay(100 / stepsPerMove);   
  }
}

void IK_BySteps_feet(double p, double o, double s, double q){
  //L1:
  double pDelta = (p - P) / stepsPerMove;
  double oDelta = (o - O) / stepsPerMove;
  //L4:
  double sDelta = (s - S) / stepsPerMove;
  double qDelta = (q - Q) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    P += pDelta; 
    O += oDelta;
    IK_feet(P, O);
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    S += sDelta; 
    Q += qDelta;
    IK_feet_L4(S, Q);
    delay(100 / stepsPerMove);   
  } 
}

//L2 & L3:
void IK_BySteps_roll_2(double n, double l, double m, double k){
  //L2:
  double nDelta = (n - N) / stepsPerMove;
  double lDelta = (l - L) / stepsPerMove;
  //L3:
  double mDelta = (m - M) / stepsPerMove;
  double kDelta = (k - K) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    N += nDelta; 
    L += lDelta;
    IK_roll_L2(N, L); 
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    M += mDelta; 
    K += kDelta;
    IK_roll_L3(M, K); 
    delay(100 / stepsPerMove);   
  } 
}

void IK_BySteps_knee_2(double j, double i, double h, double g){
  //L2:
  double jDelta = (j - J) / stepsPerMove;
  double iDelta = (i - I) / stepsPerMove;
  //L3:
  double hDelta = (h - H) / stepsPerMove;
  double gDelta = (g - G) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    J += jDelta; 
    I += iDelta;
    IK_knee_L2(J, I); 
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    H += hDelta; 
    G += gDelta;
    IK_knee_L3(H, G); 
    delay(100 / stepsPerMove);   
  } 
}

void IK_BySteps_feet_2(double f, double e, double d, double c){
  //L2:
  double fDelta = (f - F) / stepsPerMove;
  double eDelta = (e - E) / stepsPerMove;
  //L3:
  double dDelta = (d - D) / stepsPerMove;
  double cDelta = (c - C) / stepsPerMove;
  //
  for(int i = 1; i <= stepsPerMove; i++){
    F += fDelta; 
    E += eDelta;
    IK_feet_L2(F, E); 
    delay(100 / stepsPerMove);   
  } 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    D += dDelta; 
    C += cDelta;
    IK_feet_L3(D, C); 
    delay(100 / stepsPerMove);   
  } 
}

void MOVE_F(){
  //POCZATEK L1 & L4:
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512); 
  //START L1 & L4:
  IK_BySteps_roll(roll_a , roll_a_b + 512, roll_a , roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512); 
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //
  IK_BySteps_roll(roll_a , roll_a_b + 512, roll_a , roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512); //change[]
  IK_BySteps_feet(feet_a_b + 512, feet_a_b - 512, feet_a_b - 512, feet_a_b + 512); //change[]
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //
  IK_BySteps_roll(roll_a , roll_a_b + 512, roll_a , roll_a_b + 512);
  IK_BySteps_knee(knee_a_b - 512, knee_a_b + 512, knee_a_b + 512, knee_a_b - 512); //change[]
  IK_BySteps_feet(feet_a_b + 512, feet_a_b - 512, feet_a_b - 512, feet_a_b + 512); //change[]
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //KONIEC L1 & L4:
  IK_BySteps_roll(roll_a , roll_a_b + 512, roll_a , roll_a_b + 512);
  IK_BySteps_knee(knee_a_b - 512, knee_a_b + 512, knee_a_b + 512, knee_a_b - 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512);
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //
  IK_BySteps_roll(roll_a , roll_a_b + 512, roll_a , roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512);
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //
  //START L2 & L3:
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_roll_2(roll_a , roll_a_b - 512, roll_a , roll_a_b - 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512); 
  //
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_roll_2(roll_a , roll_a_b - 512, roll_a , roll_a_b - 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet_2(feet_a_b - 512, feet_a_b + 512, feet_a_b + 512, feet_a_b - 512);
  //
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_roll_2(roll_a , roll_a_b - 512, roll_a , roll_a_b - 512);
  IK_BySteps_knee_2(knee_a_b - 512, knee_a_b + 512, knee_a_b - 512, knee_a_b + 512);
  IK_BySteps_feet_2(feet_a_b - 512, feet_a_b + 512, feet_a_b + 512, feet_a_b - 512);
  //KONIEC L2 & L3:
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_roll_2(roll_a , roll_a_b - 512, roll_a , roll_a_b - 512);
  IK_BySteps_knee_2(knee_a_b - 512, knee_a_b + 512, knee_a_b - 512, knee_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512);
  //
  IK_BySteps_roll_2(roll_a , roll_a_b - 512, roll_a , roll_a_b - 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, knee_a_b + 512, knee_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, feet_a_b + 512, feet_a_b + 512);
}

void start_pos(){
  IK_BySteps_roll(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  //
  IK_BySteps_roll_2(roll_a_b + 512, roll_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_knee_2(knee_a_b + 512, knee_a_b + 512, roll_a_b + 512, roll_a_b + 512);
  IK_BySteps_feet_2(feet_a_b + 512, feet_a_b + 512, roll_a_b + 512, roll_a_b + 512);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  //
  //L1 & L4:
  L1_r.attach(3); 
  L1_k.attach(7);
  L1_f.attach(11);
  //
  L4_r.attach(4); 
  L4_k.attach(8);
  L4_f.attach(12);

  //L2 & L3:
  L2_r.attach(2); 
  L2_k.attach(6);
  L2_f.attach(10);
  //
  L3_r.attach(5); 
  L3_k.attach(9);
  L3_f.attach(13);

  pinMode(toglle, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT); 
}

void loop(){
  if(digitalRead(toglle) == 1){
    start_pos(); 
    digitalWrite(LED_BUILTIN, LOW);
  }else{
    digitalWrite(LED_BUILTIN, HIGH);
    MOVE_F(); 
  }
  delay(150); 
}