#include <Servo.h>  // Library gebruiken voor de servo

// Overal neerzetten waar ik welk pinnetje in doe
#define pinServo 5    // De servo in pin 5
#define pinButt 7     // Button in pin 7
#define pinLED1 3     // Rode/Groene led in pin 3
#define pinLED2 2     // Rode/Groene led in pin 2
int buttonState = 0;  // Op 0 zodat de button op 'uit' begint
Servo myservo;        // De servo benoemen

// Constanten verandert de waarde niet van, dit voor de sensor doen
const int TRIG_PIN = 6;             // Sensor TRIG pin in pin 6
const int ECHO_PIN = 8;             // Sensor ECHO pin in pin 8
const int LED_PIN = 9;              // Blauwe led in pin 9
const int DISTANCE_THRESHOLD = 15;  // (centimeters nog aanpassen) Is de maximale afstand tussen de sensor en een voorwerp

// Variabelen kan de waarde van veranderen
// Float is nummer met komma en kan van ver in de min tot ver in de plus
float duration_us, distance_cm;  // Variabele gemaakt voor de afstand naar de sensor en hoe lang het duurt voordat de geluidspuls terug is
//us = in microseconden en cm = centimeter lol

void setup() {
  myservo.attach(pinServo);

  pinMode(pinButt, INPUT);   // Button is de input
  pinMode(pinLED1, OUTPUT);  // De lampjes zijn de output
  pinMode(pinLED2, OUTPUT);

  Serial.begin(9600);  // Snelheid voor gegevensoverdracht in bps (bits per second)
  // Pinnetjes van de sensor als input of output benoemen
  pinMode(TRIG_PIN, OUTPUT);  // TRIG pin als output. Zendt geluid uit --> kaatst tegen voorwerp terug
  pinMode(ECHO_PIN, INPUT);   // ECHO pin als input. Ontvangt het weerkaatste geluid
  // Afstand = de tijd tussen het verzenden en ontvangen van het geluid
  pinMode(LED_PIN, OUTPUT);  // De LED is output
}

bool state = 0;  // State van te voren benoemen, bool = true or false
int degree = 0;  // Servo bij 0 laten beginnen

void loop() {
  buttonState = digitalRead(pinButt);

  if (buttonState == HIGH) {  // Button indrukken
    if (state) {
      degree -= 10;  // Hier -10 graden omdat het dan state is zoals hieronder gezegd en dan moet het dus naar 0 toe
    } else {
      degree += 10;  // Servo stokje verplaatst telkens met 10 graden
    }
    if (degree % 180 == 0) state = !state;  // Na 180 graden moet het de andere kant op naar 0
  }
  myservo.write(degree); // Servo moet aantal graden verplaatsen

  if (buttonState == LOW) {  // Als de button niet ingedrukt is, gaat het groene lampje branden
    digitalWrite(pinLED1, HIGH);
    digitalWrite(pinLED2, LOW);
  }
  if (buttonState == HIGH) {  // Als de button wel ingedrukt is, gaat het rode lampje branden
    digitalWrite(pinLED1, LOW);
    digitalWrite(pinLED2, HIGH);
  }

  // generate 10-microsecond pulse to TRIG pin
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // De variabelen meten
  // Eerst de tijd meten totdat ECHO pin geluid ontvangt en dan dus HIGH is
  duration_us = pulseIn(ECHO_PIN, HIGH);
  // Formule voor de afstand, op basis van hoe lang het geluid weg is geweest
  distance_cm = 0.017 * duration_us;

  if (distance_cm < DISTANCE_THRESHOLD) // Als de afstand kleiner is dan de afstand die ik heb aangegeven:
    digitalWrite(LED_PIN, HIGH);  // LED gaat aan
  else
    digitalWrite(LED_PIN, LOW);  // Anders staat de LED uit

  delay(100);  // De tijd tussen het opnieuw afspelen van dit circuit
}