#include <AccelStepper.h>
#include <Wire.h>

const int butver1 = 7, butver2 = 8, butver3 = 9;

const int trigpin = 12;
const int echopin = 13;
const int stepsperrot = 2038;
const int steppin = 3; 
const int dirpin = 11;

int varbut1, varbut2, varbut3;
int statebut1 = LOW, statebut2 = LOW, statebut3 = LOW;

int dtijd, afstand;

AccelStepper mystepper(AccelStepper::FULL4WIRE, steppin, dirpin); // Gebruik AccelStepper

void setup() {
  pinMode(butver1, INPUT);
  pinMode(butver2, INPUT);
  pinMode(butver3, INPUT);
  pinMode(echopin, INPUT);
  pinMode(trigpin, OUTPUT);
  mystepper.setMaxSpeed(1000);  // Stel de maximale snelheid in stappen per seconde in.
  mystepper.setSpeed(500);      // Stel de huidige snelheid in stappen per seconde in.
  Serial.begin(9600);
}

void loop() {
  digitalWrite(trigpin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigpin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigpin, LOW);

  dtijd = pulseIn(echopin, HIGH);
  afstand = (dtijd / 2) / 29.1;

  varbut1 = digitalRead(butver1);
  varbut2 = digitalRead(butver2);
  varbut3 = digitalRead(butver3);

  if (varbut1 == HIGH) {
    statebut1 = !statebut1;
    delay(200);
  }
  if (varbut2 == HIGH) {
    statebut2 = !statebut2;
    delay(200);
  }
  if (varbut3 == HIGH) {
    statebut3 = !statebut3;
    delay(200);
  }

  if (statebut1 == HIGH && afstand < 20) {
    mystepper.setSpeed(10);
    mystepper.runSpeed();  // Gebruik runSpeed om de stappenmotor te laten bewegen.
  }
  if (statebut1 == HIGH && afstand > 20) {
    mystepper.setSpeed(-10);
    mystepper.runSpeed();
  }
  if (statebut1 == HIGH && afstand == 20){
    statebut1 = !statebut1;
  }
  if (statebut2 == HIGH && afstand < 40) {
    mystepper.setSpeed(10);
    mystepper.runSpeed();
  }
  if (statebut2 == HIGH && afstand > 40) {
    mystepper.setSpeed(10);
    mystepper.runSpeed();
  }
  if(statebut2 == HIGH && afstand == 40){
    statebut2 = !statebut2;
  }
  if (statebut3 == HIGH && afstand < 80) {
    mystepper.setSpeed(10);
    mystepper.runSpeed();
  } 
  if (statebut3 == HIGH && afstand > 80) {
    mystepper.setSpeed(100);
    mystepper.runSpeed();
  } 
  if(statebut3 == HIGH && afstand == 80){
    statebut3 = !statebut3;
  }

  Serial.println(afstand);
}