#include <Servo.h>

Servo servo_leg;
Servo servo_knee;

Servo servo_leg_4; 
Servo servo_knee_4; 

int toglle = 6;

int leg_base = 90;

int knee_base = 90;

int leg_move = 150;
int leg_move_2 = 30;

int knee_move = 135;
int knee_move_2 = 45;
 
const int stepsPerMove = 20;

double X = 0;
double Y = 0;
double C = 0; 
double V = 0; 

double N = 0; 
double M = 0; 
double P = 0; 
double U = 0; 

//IK for leg
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);
}
void IK_3(double n, double m){
  double a = atan2(m, n) * (180 / 3.145);

  double l = sqrt(n * n + m * m);

  double b = atan(n / m) * (180 / 3.145);

  double a1 = b;

  servo_leg_4.write(a1);
}
//IK for knee 
void IK_2(double c, double v){
  double e = atan2(v, c) * (180 / 3.145);

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

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

  double x1 = t; 

  servo_knee.write(x1);  
}
void IK_4(double p, double u){
  double e = atan2(u, p) * (180 / 3.145);

  double r = sqrt((p * p) + (u * u));

  double t = atan(p / u) * (180 / 3.145); 

  double x1 = t; 

  servo_knee_4.write(x1);  
}
//Start for legs:
void START_P(){
  IK(90, 90);
  X = 90;
  Y = 90;
}
void START_P_1(){
  IK_3(90, 90);
  N = 90;
  M = 90;
}

//Start for knees:
void START_P_2(){
  IK_2(90, 90); 
  C = 90; 
  V = 90; 
}

void START_P_2_1(){
  IK_4(90, 90); 
  P = 90; 
  U = 90; 
}

// a function that will break the motion up into some number of steps and
// use linear interpolation to calculate each partial step.
void IK_BySteps(double x, double y){
  //
  double xDelta = (x - X) / stepsPerMove;
  double yDelta = (y - Y) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    X += xDelta;
    Y += yDelta;
    //
    IK(X, Y);
    delay(150 / stepsPerMove);
  }
}
void IK_3_BySteps(double n, double m){ 
  //
  double cDelta = (n - N) / stepsPerMove;
  double vDelta = (m - M) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    N += cDelta;
    M += vDelta;
    //
    IK_3(N, M);
    delay(150 / stepsPerMove);
  }
}
//
void IK_2_BySteps(double c, double v){
  //
  double cDelta = (c - C) / stepsPerMove;
  double vDelta = (v - V) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    C += cDelta;
    V += vDelta;
    //
    IK_2(C, V);
    delay(150 / stepsPerMove);
  }
}
void IK_4_BySteps(double p, double u){
  //
  double cDelta = (p - P) / stepsPerMove;
  double vDelta = (u - U) / stepsPerMove;
  //
  for (int i = 1; i <= stepsPerMove; i++){
    P += cDelta;
    U += vDelta;
    //
    IK_4(P, U);
    delay(150 / stepsPerMove);
  }
}
//
void MOVE_LEG_1(){
  IK_BySteps(leg_base, leg_base);
  IK_3_BySteps(leg_base, leg_base);
  delay(550); 
  IK_BySteps(leg_move, leg_base);
  IK_3_BySteps(leg_move_2, leg_base);
  delay(550); 
  IK_BySteps(leg_base, leg_base);
  IK_3_BySteps(leg_base, leg_base);
}
//
void MOVE_KNNE_1(){
  IK_2_BySteps(knee_base, knee_base); 
  IK_4_BySteps(knee_base, knee_base);
  delay(550);
  IK_2_BySteps(knee_move, knee_base); 
  IK_4_BySteps(knee_move_2, knee_base);
  delay(550);
  IK_2_BySteps(knee_base, knee_base); 
  IK_4_BySteps(knee_base, knee_base);
}
//
void setup()
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  servo_leg.attach(5);
  servo_knee.attach(3);
  servo_leg_4.attach(9);
  servo_knee_4.attach(10);
  pinMode(toglle, INPUT_PULLUP);
  pinMode(2, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop()
{
  // put your main code here, to run repeatedly:
  if (digitalRead(toglle) == 1){
    START_P();
    START_P_2(); 
    START_P_1(); 
    START_P_2_1(); 
    digitalWrite(2, HIGH);
    digitalWrite(4, LOW);
    delay(15);
  }
  else{
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
    //
    MOVE_LEG_1();
    //MOVE_KNNE_1(); 
    //COMBINE(); 
  }
}