#include <Wire.h>

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

String slovo;
String cislo;

int x = 0;
int y = 0;

int homeX = 0;
int homeY = 0;

int aktualA = 0;
int aktualB = 0;

int deltaA = 0;
int deltaB = 0;

int A = 0;
int B = 0;

int smerA = 0;
int smerB = 0;

int delayA = 0;
int delayB = 0;

bool redyA = 0;
bool redyB = 0;

String nameOfPrint;

int streamposition = 0;

int begining = 0;

int printstav = 0;

bool sdstav = 0;

#define k1 2
#define k2 3
#define k3 5
#define k4 6

void setup() {
  pinMode(pinSS, OUTPUT);
  if (SD.begin(pinCS)){sdstav = 1;}

  pinMode(k1, OUTPUT);
  pinMode(k2, OUTPUT);
  pinMode(k3, OUTPUT);
  pinMode(k4, OUTPUT);
  
  Serial.begin(9600);
  Serial.println("Arduino 2D printer");
  Serial.println("List of commands:");
  Serial.println("      move home;");
  Serial.println("      move;___;___;");
  Serial.println("      position;");
  Serial.println("      start.position;");
  Serial.println("      stop.position;");
  Serial.println("      start.print;___;");
  Serial.println("      pause.print;");
  Serial.println("      unpause.print;");
  Serial.println("      stop.print;");
  Serial.println("---------------------------------------");
  Serial.println();
  if(sdstav == 1){
  Serial.println("SD card initialization was completed");
  }else{
  Serial.println("SD card initialization faild");
  }
 Wire.begin(0x01);
  }

void loop() {



  if(Serial.available() > 0){
  slovo = Serial.readStringUntil(59);
  if(slovo == "move home"){
    x = homeX;
    y = homeY;
  }
  if(slovo == "move"){
    slovo = Serial.readStringUntil(59);
    x = slovo.toInt();
    slovo = Serial.readStringUntil(59);
    y = slovo.toInt();
    pprint();
  }
  if(slovo == "position"){
        Serial.print("position: ");
        Serial.print(aktualA);
        Serial.print("  ");
        Serial.println(aktualB);
  }
  if(slovo == "start.position"){
    streamposition = 1;
  }
  if(slovo == "stop.position"){
    streamposition = 0;
  }
  if(slovo == "start.print"){
    slovo = Serial.readStringUntil(59);
    nameOfPrint = slovo;
    printstav = 2;
    begining = 1;
  }
  if(slovo == "pause.print"){
    printstav = 1;
  }
  if(slovo == "unpause.print"){
    printstav = 2;
  }
  if(slovo == "stop.print"){
    printstav = 0;
    myFile.close();
  }
  }
  
delay(250);

if(printstav == 2){
if(begining == 1){
  myFile = SD.open(nameOfPrint);
  if (myFile){
  Serial.print("Starting of printing picture under name: ");
  Serial.println(nameOfPrint);
  }else{
  Serial.print("File ´");Serial.print(nameOfPrint);Serial.println("´ was not found");
  printstav = 0;
  }
  begining = 0;
}
if(myFile){
  cislo = myFile.readStringUntil(59);
  x = cislo.toInt();
  cislo = myFile.readStringUntil(59);
  y = cislo.toInt();
  pprint();
}
if(printstav == 2){
if(myFile.available() < 1){
  printstav = 0;
  Serial.println("Print was finished");
  myFile.close();
}}
}

if(streamposition == 1){
  Serial.print("position: ");
  Serial.print(aktualA);
  Serial.print("  ");
  Serial.println(aktualB);
}
}
void pprint(){

//-------------------------------pithágorova věta------------------------

A = x;
B = y;

//ps, zatím není žádná pithágorová věta.
//-------------------------------pithágorova věta------------------------

Serial.println(deltaA);
Serial.println(A);
Serial.println(aktualA); 
  deltaA = A - aktualA;
  deltaB = B - aktualB;

  if(deltaA < 0){
    smerA = 1;
    deltaA = abs(deltaA);
  }
  if(deltaA > 0){
    smerA = 0;
  }
  if(deltaA == 0){
    smerA = 2;
    deltaA = 1;
  }

  if(deltaB < 0){
    smerB = 1;
    deltaB = abs(deltaB);
  }
  if(deltaB > 0){
    smerB = 0;
  }
  if(deltaB == 0){
    smerB = 2;
    deltaB = 1;
  }

  if(deltaB > deltaA){
    delayB = 1;
    delayA = deltaB / deltaA;
  }
  if(deltaA > deltaB){
    delayA = 1;
    delayB = deltaA / deltaB;
  }
  if(deltaA == deltaB){
    delayA = 1;
    delayB = 1;
  }

Wire.beginTransmission(0x02);
  
Wire.write(smerB);
Wire.write(";");
Wire.write(delayB);
Wire.write(";");
Wire.write(deltaB);
Wire.write(";");
  
Wire.endTransmission();

if(smerA == 2){
  redyA = 1;
}
if(smerA == 1){
for(int i = deltaA; i > 0; i --){
protismeru();
aktualA --;
}
uvolnit();
redyA = 1;
}

if(smerA == 0){
for(int i = deltaA; i > 0; i --){
posmeru();
aktualA ++;
}
uvolnit();
redyA = 1;
}
pockejnaodpoved();
}

void pockejnaodpoved(){

cislo = Wire.readStringUntil(59);
redyB = cislo.toInt();
cislo = Wire.readStringUntil(59);
aktualB = cislo.toInt();

 delay(25);

  if(redyA == 1){
    if(redyB == 1){
      redyA = 0;
      redyB = 0;
    }else{ pockejnaodpoved();}
  }else{ pockejnaodpoved();}
  }

void posmeru(){
  digitalWrite(k1, HIGH);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
  delay(delayA);
    
  digitalWrite(k1, LOW);
  digitalWrite(k2, HIGH);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, HIGH);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, LOW);
  delay(delayA);
  
  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, LOW);
  delay(delayA);
  
  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, HIGH);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, HIGH);
  delay(delayA);

  digitalWrite(k1, HIGH);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, HIGH);
  delay(delayA);
}

void protismeru(){
  digitalWrite(k1, HIGH);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, HIGH);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, HIGH);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, HIGH);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, LOW);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, HIGH);
  digitalWrite(k3, HIGH);
  digitalWrite(k4, LOW);
  delay(delayA);

  digitalWrite(k1, LOW);
  digitalWrite(k2, HIGH);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
  delay(delayA);

  digitalWrite(k1, HIGH);
  digitalWrite(k2, HIGH);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
  delay(delayA);

  digitalWrite(k1, HIGH);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
  delay(delayA);
}

void uvolnit(){
  digitalWrite(k1, LOW);
  digitalWrite(k2, LOW);
  digitalWrite(k3, LOW);
  digitalWrite(k4, LOW);
}