#include <Servo.h>


Servo servo_leg;
Servo servo_knee;
Servo servo_leg_4; 
Servo servo_knee_4; 

Servo servo_leg_2; 
Servo servo_knee_2;
Servo servo_leg_3; 
Servo servo_knee_3; 

// - SPROBUJ NA BAZIE https://www.hackster.io/472607/quadruped-robot-gait-planning-and-application-761df2#toc-1-walk-2

int toglle = 24;

//VALUES: 

int leg_base_L1_1 = 80; 
int leg_base_L1_2 = 90; 
int knee_base_K1_1 = 50; 
int knee_base_K1_2 = 90; 
int leg_L1_M = leg_base_L1_1 - 40; 
int knee_K1_M = knee_base_K1_1 + 40; 

int leg_base_L2_1 = 120; 
int leg_base_L2_2 = 90; 
int knee_base_K2_1 = 90; 
int knee_base_K2_2 = 50; 
int leg_L2_M = leg_base_L2_1 + 45; 
int knee_K2_M = knee_base_K2_1 - 45; 

int leg_base_L3_1 = 90; 
int knee_base_K3_1 = 90; 
int knee_base_K3_2 = 110; 
int leg_L3_M = leg_base_L3_1 - 45; 
int knee_K3_M = knee_base_K3_1 + 45; 


int leg_base_L4_1 = 90; 
int knee_base_K4_1 = 90;
int leg_L4_M = leg_base_L4_1 + 40; 
int knee_K4_M = knee_base_K4_1 - 40; 

const int stepsPerMove = 40;

//Values for leg 1:
double X = 0;
double Y = 0;
//Values for knee 1:
double C = 0; 
double V = 0; 
//Leg 2:
double A = 0; 
double B = 0; 
//Knee 2: 
double D = 0; 
double F = 0; 
//Leg 3: 
double P = 0; 
double O = 0; 
//Knee 3: 
double Q = 0; 
double U = 0; 
//Leg 4: 
double T = 0; 
double R = 0; 
//Knee 4: 
double E = 0; 
double W = 0; 

//IK L1: 
void IK(double x, double y){
  double a = atan2(y, x) * (180 / 3.145);

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

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

  double a1 = b;

  servo_leg.write(a1);
}
//IK L4:
void IK_L4(double t, double r){
  double a = atan2(r, t) * (180 / 3.145);

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

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

  double a1 = b;

  servo_leg_4.write(a1);
}
//IK l2: 
void IK_L2(double a, double b){
  double v = atan2(b, a) * (180 / 3.145);

  double l = sqrt(a * a + b * b);

  double x = atan(a / b) * (180 / 3.145);

  double a1 = x;

  servo_leg_2.write(a1);
}
//IK l3: 
void IK_L3(double p, double o){
  double a = atan2(o, p) * (180 / 3.145);

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

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

  double a1 = b;

  servo_leg_3.write(a1);
}
//IK K1: 
void IK_K1(double c, double v){
  double a = atan2(v, c) * (180 / 3.145);

  double l = sqrt(c * c + v * v);

  double b = atan(c / v) * (180 / 3.145);

  double a1 = b;

  servo_knee.write(a1);
}
//IK K2: 
void IK_K2(double d, double f){
  double a = atan2(f, d) * (180 / 3.145);

  double l = sqrt(d * d + f * f);

  double b = atan(d / f) * (180 / 3.145);

  double a1 = b;

  servo_knee_2.write(a1);
}
//IK K3: 
void IK_K3(double q, double u){
  double a = atan2(u, q) * (180 / 3.145);

  double l = sqrt(q * q + u * u);

  double b = atan(q / u) * (180 / 3.145);

  double a1 = b;

  servo_knee_3.write(a1);
}
//IK K4: 
void IK_K4(double e, double w){
  double a = atan2(w, e) * (180 / 3.145);

  double l = sqrt(e * e + w * w);

  double b = atan(e / w) * (180 / 3.145);

  double a1 = b;

  servo_knee_4.write(a1);
}

//START: 

//START L1
void START_L1(){
  IK(80, 90);
  X = 80;
  Y = 90;
}
//START L2: 
void START_L2(){
  IK_L2(120, 90); 
  A = 120; 
  B = 90; 
}
//START L3: 
void START_L3(){
  IK_L3(90, 90); 
  P = 90; 
  O = 90; 
}
//START L4: 
void START_L4(){
  IK_L4(90, 90); 
  T = 90; 
  R = 90; 
}
//START K1: 
void START_K1(){
  IK_K1(50, 90); 
  C = 50; 
  V = 90; 
}
//START K2: 
void START_K2(){
  IK_K2(90, 50); 
  D = 90; 
  F = 50; 
}
//START K3: 
void START_K3(){
  IK_K3(90, 110); 
  Q = 90; 
  U = 110; 
}
//START K4: 
void START_K4(){
  IK_K4(90, 90); 
  E = 90; 
  W = 90; 
}

//SMOOTH SERVO: 
// a function that will break the motion up into some number of steps and
// use linear interpolation to calculate each partial step.

//L1 & L4:
void IK_BySteps_L1(double x, double y, double t, double r){
  //
  double xDelta = (x - X) / stepsPerMove;
  double yDelta = (y - Y) / stepsPerMove;
  double tDelta = (t - T) / stepsPerMove;
  double rDelta = (r - R) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    X += xDelta;
    Y += yDelta;
    IK(X, Y);
    delay(40 / stepsPerMove);
  }
  //
  for (int i = 1; i <= stepsPerMove; i++){
    T += tDelta;
    R += rDelta;
    IK_L4(T, R);
    delay(40 / stepsPerMove);
  }
}
//L2 & L3:
void IK_BySteps_L2(double a, double b, double p, double o){
  //
  double aDelta = (a - A) / stepsPerMove;
  double bDelta = (b - B) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    A += aDelta;
    B += bDelta;
    //
    IK_L2(A, B);
    delay(40 / stepsPerMove);
  }
  //
  double pDelta = (p - P) / stepsPerMove;
  double oDelta = (o - O) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    O += oDelta;
    P += pDelta;
    //
    IK_L3(P, O);
    delay(40 / stepsPerMove);
  }
}
//K1 & K4: 
void IK_BySteps_K1(double c, double v, double e, double w){
  //
  double cDelta = (c - C) / stepsPerMove;
  double vDelta = (v - V) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    C += cDelta;
    V += vDelta;
    //
    IK_K1(C, V);
    delay(40 / stepsPerMove);
  }
  double eDelta = (e - E) / stepsPerMove;
  double wDelta = (w - W) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    E += eDelta;
    W += wDelta;
    //
    IK_K4(E, W);
    delay(40 / stepsPerMove);
  }
}
//K2 & K3: 
void IK_BySteps_K2(double d, double f, double q, double u){
  //
  double dDelta = (d - D) / stepsPerMove;
  double fDelta = (f - F) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    D += dDelta;
    F += fDelta;
    //
    IK_K2(D, F);
    delay(40 / stepsPerMove);
  }
  double qDelta = (q - Q) / stepsPerMove;
  double uDelta = (u - U) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    Q += qDelta;
    U += uDelta;
    //
    IK_K3(Q, U);
    delay(40 / stepsPerMove);
  }
}
//PROGRAMY CHODZACE: 
//*CHODZNIE DO PRZODU*
void Krok_1(){ 
  IK_BySteps_L1(leg_base_L1_1 , leg_base_L1_2 , leg_base_L4_1 , leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1 , knee_base_K1_2 , knee_base_K4_1 , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_base_L2_1 , leg_base_L2_2 , leg_base_L3_1 , leg_base_L3_1 );  
  IK_BySteps_K2(knee_base_K2_1 , knee_base_K2_2 , knee_base_K3_1 , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_base_L1_1 , leg_base_L1_2 , leg_base_L4_1 , leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M  , knee_base_K1_2 , knee_K4_M  , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_base_L2_1 , leg_base_L2_2 , leg_base_L3_1 , leg_base_L3_1 );  
  IK_BySteps_K2(knee_base_K2_1 , knee_base_K2_2 , knee_base_K3_1 , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_L1_M  , leg_base_L1_2 , leg_L4_M  , leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M  , knee_base_K1_2 , knee_K4_M  , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_base_L2_1 , leg_base_L2_2 , leg_base_L3_1 , leg_base_L3_1 );  
  IK_BySteps_K2(knee_base_K2_1 , knee_base_K2_2 , knee_base_K3_1 , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_L1_M  , leg_base_L1_2 , leg_L4_M  , leg_base_L4_1);
  IK_BySteps_K1(knee_base_K1_1 , knee_base_K1_2 , knee_base_K4_1 , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_base_L2_1 , leg_base_L2_2 , leg_base_L3_1 , leg_base_L3_1 );  
  IK_BySteps_K2(knee_base_K2_1 , knee_base_K2_2 , knee_base_K3_1 , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_base_L1_1 , leg_base_L1_2 , leg_base_L4_1 , leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1 , knee_base_K1_2 , knee_base_K4_1 , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_base_L2_1 , leg_base_L2_2 , leg_base_L3_1 , leg_base_L3_1 );  
  IK_BySteps_K2(knee_K2_M , knee_base_K2_2  , knee_K3_M , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_base_L1_1 , leg_base_L1_2 , leg_base_L4_1 , leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1 , knee_base_K1_2 , knee_base_K4_1 , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_L2_M , leg_base_L2_2, leg_L3_M , leg_base_L3_1 ); 
  IK_BySteps_K2(knee_K2_M , knee_base_K2_2  , knee_K3_M , knee_base_K3_2 ); 
  IK_BySteps_L1(leg_base_L1_1 , leg_base_L1_2 , leg_base_L4_1 , leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1 , knee_base_K1_2 , knee_base_K4_1 , knee_base_K4_1); 
  //
  IK_BySteps_L2(leg_L2_M , leg_base_L2_2, leg_L3_M , leg_base_L3_1 ); 
  IK_BySteps_K2(knee_base_K2_1 , knee_base_K2_2 , knee_base_K3_1 , knee_base_K3_2 ); 
}
//*CHODZENIE W TYL*
void Krok_2(){
  //Start:
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);   
  //Steps: 
  IK_BySteps_L2(leg_L2_M, leg_base_L2_2, leg_L3_M, leg_base_L3_1);  
  IK_BySteps_K2(knee_K2_M, knee_base_K2_2, knee_K3_M, knee_base_K3_2);  
  IK_BySteps_L1(leg_L1_M, leg_base_L1_2, leg_L4_M, leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M, knee_base_K1_2, knee_K4_M, knee_base_K4_1);  
  //Koniec:
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);   
}
//*SKRET W PRAWO* 

int knee_K1_M_2 = knee_base_K1_1 - 40; 
int knee_K3_M_2 = knee_base_K3_1 - 65; 

void Krok_3(){
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M_2, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_K3_M_2, knee_base_K3_2);   
  //
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_K4_M, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_K2_M, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);
  //
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_L4_M, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_K4_M, knee_base_K4_1); 
  IK_BySteps_L2(leg_L2_M, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_K2_M, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2); 
  //   
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_L4_M, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_L2_M, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);    
  //   
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);    
}

//*SKRET W LEWO*

int knee_K2_M_2 = knee_base_K2_1 + 90;
int knee_K4_M_2 = knee_base_K4_1 + 90;

int leg_L1_M_2 =  leg_base_L1_1 - 45;
int leg_L3_M_2 = leg_base_L3_1 - 45; 


void Krok_4(){
  //
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_K4_M_2, knee_base_K4_1); 
  IK_BySteps_L2(leg_L2_M, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_K2_M_2, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);   
  //
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_K3_M, knee_base_K3_2);
  //
  IK_BySteps_L1(leg_L1_M_2, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_K1_M, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_L3_M_2, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_K3_M, knee_base_K3_2); 
  //   
  IK_BySteps_L1(leg_L1_M_2, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_L3_M_2, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);   
  //   
  IK_BySteps_L1(leg_base_L1_1, leg_base_L1_2, leg_base_L4_1, leg_base_L4_1);  
  IK_BySteps_K1(knee_base_K1_1, knee_base_K1_2, knee_base_K4_1, knee_base_K4_1); 
  IK_BySteps_L2(leg_base_L2_1, leg_base_L2_2, leg_base_L3_1, leg_base_L3_1);  
  IK_BySteps_K2(knee_base_K2_1, knee_base_K2_2, knee_base_K3_1, knee_base_K3_2);
}
void setup()
{
  //Legs 1 & 4:
  servo_leg.attach(2);
  servo_knee.attach(3);
  //
  servo_leg_4.attach(4); 
  servo_knee_4.attach(5); 

  //Legs 2 & 3: 
  servo_leg_2.attach(6); 
  servo_knee_2.attach(7);
  //
  servo_leg_3.attach(8); 
  servo_knee_3.attach(9);
  //Rest
  pinMode(toglle, INPUT_PULLUP);
  pinMode(LED_BUILTIN, OUTPUT);
}

void loop()
{
  // put your main code here, to run repeatedly:
  if (digitalRead(toglle) == 1){
    digitalWrite(LED_BUILTIN, LOW);
    START_L1(); 
    START_L2(); 
    START_L3(); 
    START_L4(); 
    //
    START_K1(); 
    START_K2(); 
    START_K3(); 
    START_K4(); 
  }
  else{
    digitalWrite(LED_BUILTIN, HIGH);
    Krok_1(); //w przod
    //Krok_2(); //w tyl
    //Krok_3(); //w prawo 
    //Krok_4(); //w lewo
  }
  delay(150); 
}