#include <SD.h>
#include <LiquidCrystal_I2C.h>
File myFile;
int const pinSS = A3;
int const pinCS = 10;

#define I2C_ADDR    0x27
#define LCD_COLUMNS 20
#define LCD_LINES   2

LiquidCrystal_I2C lcd(I2C_ADDR, LCD_COLUMNS, LCD_LINES);

#define ENCODER_CLK 3
#define ENCODER_DT  4
#define button      2

#define mA1 A0
#define mA2 A1
#define mA3 A2
#define mA4 5

#define mB1 6
#define mB2 7
#define mB3 8
#define mB4 9

int pismeno = 0;
int rotation = 0;
int screen = 0;
int cursor_position = 0;
bool reset = 1;
bool card_init = 0;

int speed = 1;

int A = 0;
int B = 0;

int x = 0;
int y = 0;

float necoA = 0;
float necoB = 0;

int aktualniA = 5000;
int aktualniB = 0;

int posunA = 0;
int posunB = 0;

int mode = 0;
int distance = 5000;

int smerA = 0;
int smerB = 0;

int pomerA = 0;
int pomerB = 0;

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

  lcd.init();
  lcd.backlight();

pinMode(ENCODER_CLK, INPUT);
pinMode(ENCODER_DT, INPUT);
pinMode(button, INPUT_PULLUP);

attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), readEncoder, FALLING);

pinMode(mA1, OUTPUT);
pinMode(mA2, OUTPUT);
pinMode(mA3, OUTPUT);
pinMode(mA4, OUTPUT);

pinMode(mB1, OUTPUT);
pinMode(mB2, OUTPUT);
pinMode(mB3, OUTPUT);
pinMode(mB4, OUTPUT);

pinMode(pinSS, OUTPUT);
if (SD.begin(pinCS)){
  card_init = 1;
}else{
  card_init = 0;
}
}

void readEncoder() {
  int dtValue = digitalRead(ENCODER_DT);
  if (dtValue == HIGH) {
    rotation = rotation - 1;
  }
  if (dtValue == LOW) {
    rotation = rotation + 1;
  }
}

void loop() {
if(screen == 0){
if(reset == 1){
lcd.setCursor(1,0);
lcd.print("2D printer v8");
lcd.setCursor(2,1);
lcd.print("Open source");
reset = 0;
}
if(digitalRead(button) == LOW){
  screen = 1;
  reset = 1;
  delay(300);
}
}
if(screen == 1){
  if(reset == 1){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print(">");
  rotation = 0;
  cursor_position = 0;
  reset = 0;
  }
if(cursor_position == 0){
  lcd.setCursor(1,0);
  lcd.print("Calibrate           ");
  lcd.setCursor(1,1);
  lcd.print("SD card init.       ");
  if(digitalRead(button)==LOW){
    screen = 2;
    reset = 1;
    delay(300);
  }
}
if(cursor_position == 1){
  lcd.setCursor(1,0);
  lcd.print("SD card init.       ");
  lcd.setCursor(1,1);
  lcd.print("Start printing      ");
  if(digitalRead(button)==LOW){
    screen = 5;
    reset = 1;
    delay(300);
  }
}
if(cursor_position == 2){
  lcd.setCursor(1,0);
  lcd.print("Start printing      ");
  lcd.setCursor(1,1);
  lcd.print("Move motors         ");
}
if(cursor_position == 3){
  lcd.setCursor(1,0);
  lcd.print("Move motors         ");
  lcd.setCursor(1,1);
  lcd.print("Settings            ");
  if(digitalRead(button) == LOW){
  screen = 6;
  reset = 1;
  delay(300);
}
}
if(cursor_position == 4){
  lcd.setCursor(1,0);
  lcd.print("Settings            ");
  lcd.setCursor(1,1);
  lcd.print("                    ");
}
cursor_position = rotation/5;
if(cursor_position < 0){
  cursor_position = 0;
  rotation = 0;
}
if(cursor_position > 4){
  cursor_position = 4;
  rotation = 20;
}
}
if(screen == 2){
  if(reset == 1){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("tension the wire");
  lcd.setCursor(0,1);
  lcd.print("set:");
  lcd.setCursor(8,1);
  lcd.print("act:");
  rotation = 0;
  reset = 0;
  }

lcd.setCursor(4,1);
lcd.print(rotation);
lcd.setCursor(12,1);
lcd.print(aktualniA);
mode = 3;
A = rotation;
B = rotation;
pprint();
if(digitalRead(button) == LOW){
  screen = 3;
  reset = 1;
  delay(300);
}
}
if(screen == 3){
  if(reset == 1){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("move to corner A");
  lcd.setCursor(0,1);
  lcd.print("set:");
  lcd.setCursor(8,1);
  lcd.print("act:");
  rotation = 0;
  aktualniA = 0;
  aktualniB = 0;
  reset = 0;
  }

lcd.setCursor(4,1);
lcd.print(rotation);
lcd.setCursor(12,1);
lcd.print(aktualniA);
A = rotation;
B = - rotation;
pprint();
if(digitalRead(button) == LOW){
  screen = 4;
  reset = 1;
  delay(300);
}
}
if(screen == 4){
  if(reset == 1){
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("move to corner B");
  lcd.setCursor(0,1);
  lcd.print("set:");
  lcd.setCursor(8,1);
  lcd.print("act:");
  rotation = 0;
  reset = 0;
  aktualniA = 0;
  aktualniB = 0;
  }

lcd.setCursor(4,1);
lcd.print(rotation);
lcd.setCursor(12,1);
lcd.print(aktualniA);
A = rotation;
B = - rotation;
pprint();
if(digitalRead(button) == LOW){
  aktualniB = 0;
  distance = aktualniA;
  aktualniA = aktualniB;
  aktualniB = distance;
  screen = 1;
  reset = 1;
  delay(300);
}
}
if(screen == 5){
  if(reset == 1){
  lcd.clear();
  rotation = 0;
  if (SD.begin(pinCS)){
  card_init = 1;
  }else{
  card_init = 0;
  }
  reset = 0;
  }
if(card_init == 1){
    lcd.setCursor(0,0);
    lcd.print("card is init.     ");
    lcd.setCursor(0,1);
    lcd.print("<                 ");
    if(digitalRead(button)==LOW){
      screen = 1;
      reset = 1;
      delay(300);
    }
  }else{
    lcd.setCursor(0,0);
    lcd.print("card in not init.");
    lcd.setCursor(0,1);
    lcd.print(">init");
    if(digitalRead(button)==LOW){
      if (SD.begin(pinCS)){
      card_init = 1;
      }else{
      card_init = 0;
    }
    delay(1000);
    }
  }
}
if(screen == 6){
  if(reset == 1){
  lcd.clear();
  rotation = 0;
  reset = 0;
  lcd.setCursor(1,0);
  lcd.print("mode AB");
  lcd.setCursor(1,1);
  lcd.print("mode XY");
  }
  if(rotation == 0){
    lcd.setCursor(0,0);
    lcd.print(">");
    lcd.setCursor(0,1);
    lcd.print(" ");
    if(digitalRead(button)==LOW){
      screen = 7;
      reset = 1;
      delay(300);
    }
  }
  if(rotation == 1){
    lcd.setCursor(0,0);
    lcd.print(" ");
    lcd.setCursor(0,1);
    lcd.print(">");
    if(digitalRead(button)==LOW){
      screen = 8;
      reset = 1;
      delay(300);
    }
  }
  if(rotation > 1){
    rotation = 1;
  }
  if(rotation < 0){
    rotation = 0;
  }
}
if(screen == 7){
  if(reset == 1){
  lcd.clear();
  rotation = 0;
  reset = 0;
  lcd.setCursor(1,0);
  lcd.print("A:");
  lcd.setCursor(1,1);
  lcd.print("B:");
  }
  if(pismeno == 0){
  lcd.setCursor(0,0);
  lcd.print(">");
  lcd.setCursor(3,0);
  A = rotation;
  lcd.print(A);
  lcd.print("     ");
  if(digitalRead(button)==LOW){
    pismeno = 1;
    rotation = 0;
    delay(300);
  }
  }
  if(pismeno == 1){
    lcd.setCursor(0,0);
    lcd.print(" ");
    lcd.setCursor(0,1);
    lcd.print(">");
    lcd.setCursor(3,1);
    B = rotation;
    lcd.print(B);
    lcd.print("     ");
    if(digitalRead(button)==LOW){
    delay(300);
    mode = 3;
    pprint();
    screen = 1;
    reset = 1;
    pismeno = 0;
  }
  }
}  

if(screen == 8){
  if(reset == 1){
  lcd.clear();
  rotation = 0;
  reset = 0;
  lcd.setCursor(1,0);
  lcd.print("X:");
  lcd.setCursor(1,1);
  lcd.print("Y:");
  }
  if(pismeno == 0){
  lcd.setCursor(0,0);
  lcd.print(">");
  lcd.setCursor(3,0);
  x = rotation;
  lcd.print(x);
  lcd.print("     ");
  if(digitalRead(button)==LOW){
    pismeno = 1;
    rotation = 0;
    delay(300);
  }
  }
  if(pismeno == 1){
    lcd.setCursor(0,0);
    lcd.print(" ");
    lcd.setCursor(0,1);
    lcd.print(">");
    lcd.setCursor(3,1);
    y = rotation;
    lcd.print(y);
    lcd.print("     ");
    if(digitalRead(button)==LOW){
    delay(300);
    mode = 0;
    pprint();
    screen = 1;
    reset = 1;
    pismeno = 0;
  }
  }
}  
}

void pprint(){
  ppprint();
  ppprint();
  Serial.print("aktualniA:   ");
  Serial.println(aktualniA);
  Serial.print("aktualniB:   ");
  Serial.println(aktualniB);
}
void ppprint(){

//-------------------------------pithágorova věta------------------------
if(mode == 0){
necoB = distance - x;
necoA = sqrt(y*y + x*x);
necoB = sqrt(y*y + necoB*necoB);
A = necoA;
B = necoB;

}
if(mode == 1){
A = x;
B = y;
}
//-------------------------------pithágorova věta------------------------

posunA = A - aktualniA;
posunB = B - aktualniB;

if(posunA < 0 && posunB < 0){
smerA = 1;
smerB = 1;
}

if(posunA > -1 && posunB < 0){
smerA = -1;
smerB = 1;
}

if(posunA < 0 && posunB > -1){
smerA = 1;
smerB = -1;
}

if(posunA > -1 && posunB > -1){
smerA = -1;
smerB = -1;
}

posunA = abs(posunA);
posunB = abs(posunB);

if(posunA > posunB){
pomerA = posunA/posunB;
pomerB = 1;
}
if(posunA < posunB){
pomerB = posunB/posunA;
pomerA = 1;
}
if(posunA == posunB){
  pomerA = 1;
  pomerB = 1;
}

if(posunA == 0){
  pomerA = 1;
  pomerB = posunB;
  for(int b = pomerB; b > 0; b--){
  Brotate();
  }
}

if(posunB == 0){
  pomerB = 1;
  pomerA = posunB;
  for(int a = pomerA; a > 0; a--){
  Arotate();
  }
}

while(aktualniA != A && aktualniB != B){
for(int a = pomerA; a > 0; a--){
Arotate();
}

for(int b = pomerB; b > 0; b--){
Brotate();
}
}
}

void Arotate(){
  if(smerA == -1){
    aktualniA ++;

digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, HIGH);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);

  }
  if(smerA == 1){
    aktualniA --;
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, HIGH);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, HIGH);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, HIGH);
digitalWrite(mA2, HIGH);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, HIGH);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
delay(speed);
digitalWrite(mA1, LOW);
digitalWrite(mA2, LOW);
digitalWrite(mA3, LOW);
digitalWrite(mA4, LOW);
  }

}
void Brotate(){
  if(smerB == -1){
    aktualniB ++;

digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);

  }
  if(smerB == 1){
    aktualniB --;
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, HIGH);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, HIGH);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, HIGH);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, HIGH);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
delay(speed);
digitalWrite(mB1, LOW);
digitalWrite(mB2, LOW);
digitalWrite(mB3, LOW);
digitalWrite(mB4, LOW);
}}