//#include <SoftwareSerial.h>

int DIR_X_PIN = 2;
int STEP_X_PIN = 3;

int Chapac_PIN = 4;

int DIR_Y_PIN = 5;
int STEP_Y_PIN = 6;

int DIR_PIN = DIR_X_PIN;
int STEP_PIN = STEP_X_PIN;

int keyStart = A0; // PIN pro připojení tlačítka Start
int keyHome = A1; // PIN pro připojení tlačítka Home

int limit_XL_Refer = 1; // PIN pro připojení tlačítka Refer osy X leva strana
int limit_YP_Refer = 9; // PIN pro připojení tlačítka Refer osy Y predni snimac

int stoupani_sroubu_X = 8; 
int kroku_na_otacku_X = 200;

int stoupani_sroubu_Y = 8; 
int kroku_na_otacku_Y = 200;


float Xcounter = 0;
float Ycounter = 0;
float Zcounter = 0;

void setup() {
pinMode(DIR_X_PIN, OUTPUT);
pinMode(STEP_X_PIN, OUTPUT);
pinMode(DIR_Y_PIN, OUTPUT);
pinMode(STEP_Y_PIN, OUTPUT);
pinMode(Chapac_PIN, OUTPUT);

pinMode(keyStart, INPUT_PULLUP); // tady připojuješ tlačítko na pullup.
pinMode(keyHome, INPUT_PULLUP); //tlačítko Home
pinMode(limit_XL_Refer, INPUT_PULLUP); //referenční snímač 1 na ose x

Serial.begin(9600);

}

void loop() {
if (digitalRead(keyStart) == LOW) {
  delay(500);
  if (digitalRead(keyStart) == LOW) {
    moveX(400, 0.07);               // přesun na 400mm rychlosti 0.07
    chapac(1);                      // zavreni chapadla
    moveX(-400, 0.7);
    chapac(0);                      // otevření chapadla
    moveY(200,0.7);
    }
  }
  
if (digitalRead(keyHome) == LOW) {
  delay(500);
  if (digitalRead(keyHome) == LOW) {
    moveX(-100, 0.3); //Hledání výchozí polohy.
    moveY(-100, 0.3);
  }
  }
  }


// functions
void moveX(int posun, float rychlost){
  int Xcurrent = 1;
  int Bx = Xcounter-posun;
  Bx = Bx*(-1);
  int Ot = Bx * (kroku_na_otacku_X/stoupani_sroubu_X);
  int zrychleni = 400; // tady se mění zrychlení motoru při rozjezdu (počet kroků na plnou rychlost)
  float maxDelay = 700; // tady se mění prudkost náběhu motoru (když dělá velký rázy, zvýší se číslo)
  float Ost = 1/(kroku_na_otacku_X/stoupani_sroubu_X);
  rotate(Ot, rychlost, DIR_X_PIN, STEP_X_PIN, Xcurrent, zrychleni, maxDelay, Ost);
  }
  
void moveY(int posun, float rychlost){
  int Ycurrent = 2;
  int By = Ycounter-posun;
  By = By*(-1);
  int Ot = By * (kroku_na_otacku_Y/stoupani_sroubu_Y);
  int zrychleni = 400; // tady se mění zrychlení motoru při rozjezdu (počet kroků na plnou rychlost)
  float maxDelay = 700; // tady se mění prudkost náběhu motoru (když dělá velký rázy, zvýší se číslo)
  float Ost = 1/(kroku_na_otacku_Y/stoupani_sroubu_Y);
  rotate(Ot, rychlost, DIR_Y_PIN, STEP_Y_PIN, Ycurrent, zrychleni, maxDelay, Ost);
  }
  
  
  
  
void rotate(int steps, float speed, int DIR_PIN, int STEP_PIN, int Current, int zrychleni, float maxDelay, float Ost) {
//rotate a specific number of steps - (negitive for reverse movement)
//speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
int dir = (steps > 0) ? HIGH : LOW;

steps = abs(steps);


digitalWrite(DIR_PIN, dir);
int stepsStart = zrychleni;
int stepsMid = steps-zrychleni*2;
int stepsEnd = zrychleni;
if (dir == LOW) Ost = Ost*(-1);
float Counter = Zcounter;
if (Current == 1) float Counter = Xcounter;  
if (Current == 2) float Counter = Ycounter;

float oneStep = maxDelay/zrychleni;
float usDelay = (1 / speed) * 70;

if (steps < 2){
  limits(); // kontrola jestli se má něco řešit před dalším pohybem
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(usDelay);
  
  digitalWrite(STEP_PIN, LOW);
  float Counter = Counter + Ost;
  counters();
  delayMicroseconds(usDelay);
  return;
  }

if (steps <=(zrychleni*2)+1){
  int stepsMod = steps % 2;
  int stepsDiv = steps-stepsMod;
  stepsStart = (stepsDiv/2);
  stepsEnd = stepsDiv/2;
  stepsMid = stepsMod;
  }

for (int i = 0; i <= stepsStart; i++){ //rozjezd motoru
 limits();
 digitalWrite(STEP_PIN, HIGH);
 delayMicroseconds(usDelay);
 
 digitalWrite(STEP_PIN, LOW);
 float Counter = Counter + Ost;
 counters();
 delayMicroseconds(usDelay+(maxDelay-oneStep*i));
 }

for (int i = 0; i <= stepsMid; i++) { // středová vzdálenost
  limits();
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(usDelay);

  digitalWrite(STEP_PIN, LOW);
  Counter += Ost;
  counters();
  delayMicroseconds(usDelay);
  }

for (int i = 0; i <= stepsEnd; i++){ //zastavení motoru
  limits();
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(usDelay);

  digitalWrite(STEP_PIN, LOW);
  Counter += Ost;
  counters();
  delayMicroseconds(usDelay+(oneStep*i));
  }
  if (Current ==1) Xcounter = Counter;
  if (Current ==2) Ycounter = Counter;
  }

void rotateDeg(float deg, float speed) {
//rotate a specific number of degrees (negitive for reverse movement)
//speed is any number from .01 -> 1 with 1 being fastest - Slower is stronger
int dir = (deg > 0) ? HIGH : LOW;
digitalWrite(DIR_PIN, dir);

int steps = abs(deg) * (1 / 0.225);
float usDelay = (1 / speed) * 70;

for (int i = 0; i < steps; i++) {
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(usDelay);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(usDelay);
  }
  }

void limits(){
// if (koncák 1 tak...
// if (koncák 2 tak...
// if (spínač 1 tak...
if (digitalRead(limit_XL_Refer) == LOW) {
  while (digitalRead(limit_XL_Refer) == LOW) {
    digitalWrite(DIR_X_PIN, HIGH);
    digitalWrite(STEP_X_PIN, HIGH);
    delayMicroseconds(5);
    digitalWrite(STEP_X_PIN, LOW);
    delay(1);    
    }
  digitalWrite(DIR_X_PIN, LOW);
  Xcounter = 0;
  loop();
  }
}


void chapac(int command){
  if (command == 1) digitalWrite(Chapac_PIN, HIGH); // sevreni chapadla
  if (command == 0) digitalWrite(Chapac_PIN, LOW);  // uvolneni chapadla
  }

void counters(){ // odesilani udaju o poloze
  Serial.print("Y:");
  Serial.print(Ycounter);
  Serial.print(" X:");
  Serial.print(Xcounter);
  Serial.print(" Z:");
  Serial.println(Zcounter);
  }