#include <Stepper.h>
#include <LiquidCrystal_I2C.h>
#include <Wire.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

#define pizza 4
#define ehoPin 2
#define trigPin 3
#define redPin 7
#define greenPin 6

long vr_dist = 0;
int step = 0;
long cur_step = 0;
//tec
const int stepsPerRevolution = 12;  

Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);

unsigned long sek;
unsigned long doli_sek;
bool gorit = false;

void setup() {
  pinMode(trigPin, OUTPUT);
  pinMode(ehoPin, INPUT);
  pinMode(redPin, OUTPUT);
  pinMode(greenPin, OUTPUT);
  pinMode(pizza, OUTPUT);

  myStepper.setSpeed(120);
  Serial.begin(9600);

  lcd.init();
  lcd.backlight();
  print_to_lcd(0, 0, "Step: " + String(0) + " ");
  //cor
  print_to_lcd(0, 1, "Dist: " + String(0) + " cm    ");

}


void loop() {
  long duration;
  long distance = 0;
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10); 
  digitalWrite(trigPin, LOW);
  duration = pulseIn(ehoPin, HIGH);
  distance = (duration / 2) / 29.1;
  if(distance < 2) {
    distance = 2; 
  }

  if(vr_dist != distance) {
    print_to_lcd(0, 1, "Dist: " + String(distance) + " cm    ");
    digitalWrite(greenPin, LOW);
    tone(pizza, 500);
    int raznica = distance - vr_dist;
    int one_step = 1;
    if(raznica < 0) {
      one_step = -1;
    }
    else{
      one_step = 1;
    }
    
    while(step < abs(raznica)) {
      myStepper.step(one_step);
      step++;
      redMig();
      cur_step += 1 * one_step;
      print_to_lcd(0, 0, "Step: " + String(cur_step) + " ");
      print_to_lcd(10, 0, "MOVING");
    }
    step = 0;
    vr_dist = distance;
    print_to_lcd(10, 0, "READY  ");
    noTone(pizza);
    digitalWrite(redPin, LOW);
  }
  digitalWrite(greenPin, HIGH);
}

void print_to_lcd(int x, int y, String stroka) {
  lcd.setCursor(x, y);
  lcd.print(stroka);
}

void redMig() {
  if(millis() - sek >= 1000) {
    sek = millis();
    digitalWrite(redPin, HIGH);
    doli_sek = millis();
    gorit = true;
  }
  if(gorit && millis() - doli_sek >= 200) {
    gorit = false;
    doli_sek = millis();
    digitalWrite(redPin, LOW);
  }
}