#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Stepper.h>
#define FULLSTEP 2038
Stepper stepper(FULLSTEP, 3, 4, 5, 6);

// горизонт
const int stepXPin = 2;
const int dirXPin  = 5;

// вилка
const int stepYPin = 3;
const int dirYPin  = 6;
// вертикаль
const int stepZPin = 4;
const int dirZPin  = 7;

//const int enPin = 8;
//const int homeSwitchPin = 7;

char receivedChar;
boolean newData = false;
// LCD
LiquidCrystal_I2C lcd(0x27, 16, 2);

int upButton = 10;
int downButton = 11;
int selectButton = 12;
int menu = 1;

void setup() {
  
  pinMode(stepXPin,OUTPUT);
  pinMode(dirXPin,OUTPUT);

  pinMode(stepYPin,OUTPUT);
  pinMode(dirYPin,OUTPUT);

  pinMode(stepZPin,OUTPUT);
  pinMode(dirZPin,OUTPUT);

  pinMode(upButton, INPUT);
  pinMode(downButton, INPUT);
  pinMode(selectButton, INPUT);

  digitalWrite(dirYPin,LOW); 
  lcd.init();
  lcd.backlight();

  updateMenu();

Serial.begin(9600);
  stepper.setSpeed(15);
}

int istep = 2500;
int state = 0;
int numDelay = 500;
int xstep = 1000;
int zstep =  500;

void loop() { 
  if (digitalRead(downButton)){
    menu++;
    updateMenu();
    delay(100);
  }
  if (digitalRead(upButton)){
    menu--;
    updateMenu();
    delay(100);
  }
  if (digitalRead(selectButton)){
    executeAction();
    updateMenu();
    delay(100);
  }
}

void motorStep( int MAX){
  for(int x = 0; x < MAX; x++) 
  {
    digitalWrite(stepYPin,HIGH);
    delayMicroseconds(numDelay);
    digitalWrite(stepYPin,LOW);
    delayMicroseconds(numDelay);
  }

}

void motorXStep( int MAX){
  for(int x = 0; x < MAX; x++) 
  {
    digitalWrite(stepXPin,HIGH);
    delayMicroseconds(500);
    digitalWrite(stepXPin,LOW);
    delayMicroseconds(500);
  }
}
void motorZStep( int MAX){
  for(int x = 0; x < MAX; x++) 
  {
    digitalWrite(stepZPin,HIGH);
    delayMicroseconds(500);
    digitalWrite(stepZPin,LOW);
    delayMicroseconds(500);
  }
}

void updateMenu() {
  switch (menu) {
    case 0:
    menu = 1;
    break;
    case 1:
    lcd.clear();
    lcd.print(">UNIT1 UNIT2");
    lcd.setCursor(0, 1);
    lcd.print(" UNIT3 PLACE->");

    break;
    case 2:
    lcd.clear();
    lcd.print(" UNIT1>UNIT2");
    lcd.setCursor(0, 1);
    lcd.print(" UNIT3 PLACE->");

    break;
    case 3:
    lcd.clear();
    lcd.print(" UNIT1 UNIT2");
    lcd.setCursor(0, 1);
    lcd.print(">UNIT3 PLACE->");

    break;

    case 4:
    lcd.clear();
    lcd.print("<-RETRIVE >UNIT1");
    lcd.setCursor(0, 1);
    lcd.print(" UNIT2 UNIT3");

    break;
    case 5:
    lcd.clear();
    lcd.print("<-RETRIVAL UNIT1");
    lcd.setCursor(0, 1);
    lcd.print(">UNIT2 UNIT3");

    break;
    case 6:
    lcd.clear();
    lcd.print("<-RETRIVAL UNIT1");
    lcd.setCursor(0, 1);
    lcd.print(" UNIT2>UNIT3");

    case 7:
    menu = 6;

    break;
  }
}

void executeAction() {
  switch (menu) {
    case 1:
    unit1retrive();
    break;
    case 2:
    unit2retrive();
    break;
    case 3:
    unit3retrive();
    break;
    case 4:
    unit1place();
    break;
    case 5:
    unit2place();
    break;
    case 6:
    unit3place();
    break;
  }
}

void unit1retrive() {
  digitalWrite(dirXPin,LOW);
  motorXStep(3000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000);
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);

  digitalWrite(dirXPin,HIGH);
  motorXStep(3000);
  delay(1000); 

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
}
void unit2retrive() {
    digitalWrite(dirXPin,LOW);
  motorXStep(2000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000);
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);

  digitalWrite(dirXPin,HIGH);
  motorXStep(2000);
  delay(1000); 

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
}
void unit3retrive() {
    digitalWrite(dirXPin,LOW);
  motorXStep(1000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000);
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);

  digitalWrite(dirXPin,HIGH);
  motorXStep(1000);
  delay(1000); 

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
}


void unit1place() {
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,LOW);
  motorXStep(3000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000); 

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,HIGH);
  motorXStep(3000);
  delay(1000); 

  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);
}
void unit2place() {
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,LOW);
  motorXStep(2000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000);

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,HIGH);
  motorXStep(2000);
  delay(1000); 

  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);
}
void unit3place() {
//taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,LOW);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,LOW);
  motorXStep(1000);
  delay(1000); 

  digitalWrite(dirZPin,LOW);
  motorZStep(3000);
  delay(1000); 

  //taker
  state = 1;
  istep = 250; 
  digitalWrite(dirYPin,LOW);
  motorStep(istep);

  digitalWrite(dirZPin,HIGH);  
  motorZStep(100);
  delay(1000);

  state = 2;
  istep = 250; 
  digitalWrite(dirYPin,HIGH);
  motorStep(istep);
//takerend
  digitalWrite(dirXPin,HIGH);
  motorXStep(1000);
  delay(1000); 

  digitalWrite(dirZPin,HIGH);
  motorZStep(3000);
  delay(1000);
}