#include <Arduino.h>
#include "BasicStepperDriver.h"

#define motorSteps 200
#define RPM 120
#define microsteps  2
#define dir  25
#define step  26

BasicStepperDriver stepper(motorSteps,dir,step);



//buttons Motor bewegen
int start_motor = 34;
int stop_motor = 35;
int back_motor = 17;
int lastbuttonstate_start;
int currentbuttonstate_start;
int lastbuttonstate_stop;
int currentbuttonstate_stop;
int currentbuttonstate_back;
int lastbuttonstate_back;
boolean check;
boolean check2;

//Ultraschall Integers
int trigger = 23;
int echo = 22;

//Ultraschallsignal 1 speichern
int safe_echo = 32;
int lastbuttonstate_safe;
int currentbuttonstate_safe;
int duration_safe;

//Memorybutton und Integer für 2. Ultraschallsignal
int memory_echo = 33;
int lastbuttonstate_memory;
int currentbuttonstate_memory;
int duration_memory;


void setup() {


  Serial.begin(9600);
  //Steppermotorboard 
  pinMode(step, OUTPUT); //Step pin as output
  pinMode(dir,  OUTPUT); //Direcction pin as output
  digitalWrite(step, LOW); // Currently no stepper motor movement
  digitalWrite(dir,LOW);
  

  // Set target motor RPM to and microstepping setting
  stepper.begin(RPM, microsteps);
  
  //Buttons für normales Verfahren des Motors
  pinMode(start_motor, INPUT);
  pinMode(stop_motor, INPUT);
  pinMode(back_motor, INPUT);
  //Ausgangssituation
  currentbuttonstate_start = digitalRead(start_motor);
  currentbuttonstate_stop = digitalRead(stop_motor);
  currentbuttonstate_back = digitalRead(back_motor);
  check = false;
  check2 = false;

  //Ultraschallsensor
  pinMode(safe_echo,INPUT);
  pinMode(trigger,OUTPUT);

  //Button zum speichern des ursrpünglichen Wertes
  pinMode(echo, INPUT);
  currentbuttonstate_safe = digitalRead(safe_echo);
 
  //Button für die Memoryfahrt
  pinMode(memory_echo,INPUT);
  currentbuttonstate_memory = digitalRead(memory_echo);
}
 
void loop() 
{
  //Notwendig, um die Buttons als Schalter im Loop zu verwenden
  lastbuttonstate_start = currentbuttonstate_start;
  currentbuttonstate_start = digitalRead(start_motor);
  lastbuttonstate_stop = currentbuttonstate_stop;
  currentbuttonstate_stop = digitalRead(stop_motor);
  lastbuttonstate_back = currentbuttonstate_back;
   currentbuttonstate_back = digitalRead(back_motor);
  lastbuttonstate_safe = currentbuttonstate_safe;
  currentbuttonstate_safe = digitalRead(safe_echo);
  lastbuttonstate_memory = currentbuttonstate_memory;
  currentbuttonstate_memory = digitalRead(memory_echo);

  //Der Boolean Check wird über die beiden Taster auf true und false gesetzt. Wenn der 
  //Boolean auf true gesetzt ist, fährt der Motor. Deswegen wird der Boolean im Setup auch 
  //erst auf false gesetzt, damit der Motor zu Beginn nicht fährt. 
  if(lastbuttonstate_start == HIGH  && currentbuttonstate_start == LOW)
  {
     check = true;
     check2 = false;
  }
  if(lastbuttonstate_stop == HIGH && currentbuttonstate_stop == LOW)
  {
     check = false;
     check2 = false;
  }
  if(digitalRead(21) == HIGH)
    {
      check == false;
    }
  if(check == true)
  {
  
    stepper.rotate(5);
    delay(100);
  
  }
  if(lastbuttonstate_back == HIGH && currentbuttonstate_back == LOW)
  {
    check2 = true;
    check = false;
  }
  if(check2 == true)
  {
    stepper.rotate(-5);
    delay(100);
  }

  
  //Wenn der safe_echo-Button gedrückt wird, soll der aktuelle Wert des Ultraschalls gespeichert werden.
  if(lastbuttonstate_safe == HIGH && currentbuttonstate_safe == LOW)
  {
  // Initiator für eine Ultraschallmessung
  digitalWrite(trigger, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigger, LOW);

  // Speichern des Wertes der serial.print dient zur Kontrolle hier
  duration_safe = pulseIn(echo, HIGH);
  Serial.print("Distance in CM: ");
  Serial.println(duration_safe / 58);
  delay(1000);
  }

  //Wird der Memory-Button getätigt, wird der Motor solange verfahren, bis der Wert des Safe buttons erreicht wurde. 
  if(lastbuttonstate_memory == HIGH && currentbuttonstate_memory == LOW)
  {
    if(duration_safe != NULL)
    {
      digitalWrite(trigger, HIGH);
      delayMicroseconds(10);
      digitalWrite(trigger, LOW);
      //Code um zu beschreiben, dass der Motor solange fahren soll, bis die Höhe X des Sensors erreicht wurde
      duration_memory = pulseIn(echo, HIGH);
  
      if(duration_memory > duration_safe)
      {
        
        while(duration_memory > duration_safe)
        {
          digitalWrite(trigger, HIGH);
          delayMicroseconds(10);
          digitalWrite(trigger, LOW);
          duration_memory = pulseIn(echo, HIGH);

          stepper.rotate(-5);
        }
      }
        
      if(duration_memory < duration_safe)
      { 
        while(duration_memory < duration_safe)
          {
            digitalWrite(trigger, HIGH);
            delayMicroseconds(10);
            digitalWrite(trigger, LOW);
            duration_memory = pulseIn(echo, HIGH);
            stepper.rotate(5);
          }
      }
      
    }
    
  }

}