#include <Servo.h>
Servo servo_leg; 
Servo servo_knee;
const int toglle = 6; 
//
int IK(double x, double y, double z, double v){

  double a = atan2(y, x) * (180 / 3.145); 
  double a_2 = atan2(v, z) * (180 / 3.145);

  double l = sqrt(x * x + y * y); 
  double l_2 = sqrt(z * z + v * v); 

  double b = atan(x / y) * (180 / 3.145); 
  double b_2 = atan(z / v) * (180 / 3.145); 
  //Serial.print("b(w radianach):");
  //Serial.println(b);
  //delay(300);
  //Serial.print("b_2(w radianach):");
  //Serial.println(b_2);

  double a1 = b; 
  double b1 = b_2; 

  servo_leg.write(a1); 
  servo_knee.write(b1); 
}
//
void MOVE(){
  IK(90, 90, 0, 0); //!
  Serial.println("BAZA || BASE POSITION");
  Serial.println();
  delay(1000); 
  IK(90, 90, 15, 45); //!
  Serial.println("KOLANO GORA || KNEE UP");
  Serial.println();
  delay(1000); 
  IK(180, 90, 15, 45); //!
  Serial.println("NOGA W LEWO || LEG TO THE LEFT");
  Serial.println();
  delay(1000);
  IK(180, 90, 0, 0); //!
  Serial.println("KOLANO DOL || KNEE DOWN");
  Serial.println();
  delay(1000);
}
//
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servo_leg.attach(5); 
  servo_knee.attach(3); 
  pinMode(toglle, INPUT_PULLUP);
  pinMode(2, OUTPUT); 
}

void loop() {
  // put your main code here, to run repeatedly:
  if(digitalRead(toglle) == HIGH){
    IK(90, 90, 0, 0); 
    digitalWrite(2, HIGH);
  }else{
     digitalWrite(2, LOW);
    MOVE(); 
  }
}