int x = 0;
int y = 0;

int A = 0;
int B = 0;

int Apomer = 0;
int Bpomer = 0;

int Aaktualni = 0;
int Baktualni = 0;

int smerA = 0;
int smerB = 0;

int Bprosteneco = 0;
int Aprosteneco = 0;

float prom = 0;

int zbyvajiciKrok = 0;

int speed = 0;

int paper = 0;

int KA1 = 2;
int KA2 = 3;
int KA3 = 4;
int KA4 = 5;

int KB1 = 6;
int KB2 = 7;
int KB3 = 8;
int KB4 = 9;

int Bdown = 10;
int Bup = 11;
int Adown = 12;
int Aup = 13;

void setup() {
  Serial.begin(9600);

  pinMode(KA1, OUTPUT);
  pinMode(KA2, OUTPUT);
  pinMode(KA3, OUTPUT);
  pinMode(KA4, OUTPUT);

  pinMode(KB1, OUTPUT);
  pinMode(KB2, OUTPUT);
  pinMode(KB3, OUTPUT);
  pinMode(KB4, OUTPUT);

  pinMode(Bdown, INPUT_PULLUP);
  pinMode(Bup, INPUT_PULLUP);
  pinMode(Adown, INPUT_PULLUP);
  pinMode(Aup, INPUT_PULLUP);

}

void loop() {
delay(500);
  if(digitalRead(Adown) == LOW){
    manual();
  }

//---------------------------------------------------------------------  

  speed = 1;

  paper = 210; //mm

  x = 200;
  y = 150;
  pprint();
  ppprint();

  x = 40;
  y = 100;
  pprint();
  ppprint();

  x = 120;
  y = 200;
  pprint();
  ppprint();

  x = 100;
  y = 20;
  pprint();
  ppprint();

  x = 100;
  y = 100;
  pprint(); 
  ppprint();

  x = 0;
  y = 0;
  pprint(); 
  ppprint();

//---------------------------------------------------------------------

  konec();
  delay(30000);
}

void konec(){
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);

  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
}

void pprint(){

A = sqrt(x*x + y*y);
B = sqrt(y*y + (paper - x)*(paper - x));

  A = Aaktualni - A;
  if(A < 0){smerA = 1;}else{smerA = 0;}
  A = abs(A);

  B = Baktualni - B;
  if(B < 0){smerB = 1;}else{smerB = 0;}
  B = abs(B);

 if(A == 0){A = 1;}
 if(B == 0){B = 1;}

if(A > B){
prom = A / 10;
Apomer = A / prom;
Bpomer = B / prom;
}else{

if(B > A){
prom = B / 10;
Apomer = A / prom;
Bpomer = B / prom;
}else{

if(A == B){
Apomer = 1;
Bpomer = 1;
}
}
}

zbyvajiciKrok = prom;

rotace();

}





void manual(){
  if(digitalRead(Aup) == LOW){
    digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, HIGH);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(1);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(1);
  }

  if(digitalRead(Adown) == LOW){
    digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, HIGH);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(1);
  }

if(digitalRead(Bup) == LOW){
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, HIGH);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(1);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(1);
}

  if(digitalRead(Bdown) == LOW){

  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, HIGH);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(1);    
  }

if(digitalRead(Aup) != LOW && digitalRead(Adown) != LOW && digitalRead(Bup) != LOW && digitalRead(Bdown) != LOW){
  konec();
}
  
  manual();
}
//-----------------------------------------------------------------
void rotace(){
  zbyvajiciKrok = zbyvajiciKrok - 1;
  if(smerA == 1){
    Aprosteneco = Apomer;
    Aprotismeru();
  }else{
    Aprosteneco = Apomer;
    Aposmeru();
  }

  if(smerB == 1){
    Bprosteneco = Bpomer;
    Bprotismeru();
  }else{
    Bprosteneco = Bpomer;
    Bposmeru();
  }
  if(zbyvajiciKrok > 0){
    rotace();
  }
}

void Aposmeru(){
  Aprosteneco = Aprosteneco - 1;
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, HIGH);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(speed);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(speed);

  Aaktualni = Aaktualni - 1;
  if(Aprosteneco <= 0){
  }else{
    Aposmeru();
  }
}

void Aprotismeru(){
  Aprosteneco = Aprosteneco - 1;
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, HIGH);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, HIGH);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, HIGH);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, LOW);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, HIGH);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);
  digitalWrite(KA1, HIGH);
  digitalWrite(KA2, LOW);
  digitalWrite(KA3, LOW);
  digitalWrite(KA4, LOW);
  delay(speed);

  Aaktualni = Aaktualni + 1;
  if(Aprosteneco <= 0){
  }else{
    Aprotismeru();
  }
}

void Bposmeru(){
  Bprosteneco = Bprosteneco - 1;
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, HIGH);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(speed);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(speed);

  Baktualni = Baktualni - 1;
  if(Bprosteneco <= 0){
  }else{
    Bposmeru();
  }
}

void Bprotismeru(){
  Bprosteneco = Bprosteneco - 1;
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, HIGH);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, HIGH);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, HIGH);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, LOW);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, HIGH);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);
  digitalWrite(KB1, HIGH);
  digitalWrite(KB2, LOW);
  digitalWrite(KB3, LOW);
  digitalWrite(KB4, LOW);
  delay(speed);

  Baktualni = Baktualni + 1;
  if(Bprosteneco <= 0){
  }else{
    Bprotismeru();
  }
}

void ppprint(){
  Serial.print(Aaktualni);
  Serial.print("  ");
  Serial.println(Baktualni);
}