#include <Servo.h>

// Definieer de servo's
Servo servo1; // Bakje servo
Servo servo2; // Kap servo

// Posities in graden voor servo's
int pos1 = 0;    // Positie 1: Rustpositie (startpositie)
int pos2 = 45;   // Positie 2: Bakje sluiten (kap sluit over bakje)
int pos3 = 90;   // Positie 3: Beide servo's naar uitgangspositie
int pos4 = 135;  // Positie 4: Bakje opent om de bal uit te werpen
int pos5 = 180;  // Positie 5: Bal wordt uitgeworpen

// Posities voor uitwerpen van specifieke kleuren
int pos_blue = 45;   // Positie voor blauw
int pos_green = 90;  // Positie voor groen
int pos_red = 135;   // Positie voor rood

// Functie om kleur te detecteren (mock-up)
String detectColor() {
  // Simuleer kleurendetectie
  int randomColor = random(0, 3);
  if (randomColor == 0) return "blue";
  else if (randomColor == 1) return "green";
  else return "red";
}

void setup() {
  // Initialiseer de servo's
  servo1.attach(9);  // Servo 1 (Bakje) verbonden met digitale pin 9
  servo2.attach(10); // Servo 2 (Kap) verbonden met digitale pin 10

  // Serial monitor starten
  Serial.begin(9600);
  Serial.println("Start programma...");

  // Startpositie (servo1 in pos2 en servo2 in pos1)
  servo1.write(pos2);  // Bakje draait naar positie 2 (start voor object)
  servo2.write(180 - pos1);  // Kap in positie 1 (ondersteboven)
  Serial.println("Servo's naar beginpositie.");
  delay(5000); // Wacht 5 seconden
}

void loop() {
  // Stap 1: Voorwerp in het bakje brengen
  Serial.println("Voorwerp komt in het bakje...");
  servo1.write(pos2);  // Bakje houdt object vast
  servo2.write(180 - pos1);  // Kap in rustpositie (positie 1 ondersteboven)
  delay(5000);  // Wacht 5 seconden

  // Stap 2: Kap sluit om kleur te meten
  Serial.println("Kap sluit voor kleurmeting...");
  servo2.write(180 - pos2);  // Kap sluit over bakje in positie 2 ondersteboven
  delay(5000);  // Wacht 5 seconden voor kleurmeting

  // Stap 3: Kleur detecteren
  String color = detectColor();
  Serial.print("Gedetecteerde kleur: ");
  Serial.println(color);

  // Stap 4: Beide servo's bewegen naar de juiste uitgangspositie
  Serial.println("Beide servo's naar uitgangspositie...");
  servo1.write(pos3);  // Bakje beweegt naar uitgangspositie
  servo2.write(180 - pos3);  // Kap beweegt naar uitgangspositie ondersteboven
  delay(5000);  // Wacht 5 seconden voor het uitleiden van de bal

  // Stap 5: Het object uitwerpen op basis van de kleur
  if (color == "blue") {
    Serial.println("Object wordt uitgeworpen naar blauw gebied...");
    servo1.write(pos_blue + 45);  // Bakje draait verder dan het kapje naar de blauwe uitwerppositie
    servo2.write(180 - pos_blue);  // Kap blijft op zijn positie
  } 
  else if (color == "green") {
    Serial.println("Object wordt uitgeworpen naar groen gebied...");
    servo1.write(pos_green + 45);  // Bakje draait verder dan het kapje naar de groene uitwerppositie
    servo2.write(180 - pos_green);  // Kap blijft op zijn positie
  } 
  else if (color == "red") {
    Serial.println("Object wordt uitgeworpen naar rood gebied...");
    servo1.write(pos_red + 45);  // Bakje draait verder dan het kapje naar de rode uitwerppositie
    servo2.write(180 - pos_red);  // Kap blijft op zijn positie
  }
  delay(5000);  // Wacht 5 seconden voor het uitwerpen van de bal

  // Terug naar startpositie (servo 1 in pos2, servo 2 in pos1)
  Serial.println("Terug naar startpositie...");
  servo1.write(pos2);  // Bakje draait terug naar positie 2
  servo2.write(180 - pos1);  // Kap draait terug naar positie 1 ondersteboven
  delay(5000);  // Wacht 5 seconden
}