// Pins bipolar stepper motor 1
const int stepPin1 = 3;
const int dirPin1 = 4;

// Pins bipolar stepper motor 2
const int stepPin2 = 5;
const int dirPin2 = 6;

// LDR sensor pin
const int ldrPin = A0;

// Aantal stappen naar containers
const int stepsToContainerBlue = 200;
const int stepsToContainerRed = 400;
const int stepsToContainerGreen = 600;

void setup() {
  Serial.begin(9600);

  pinMode(stepPin1, OUTPUT);
  pinMode(dirPin1, OUTPUT);

  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);

  pinMode(ldrPin, INPUT);
}

void loop() {
  Serial.println("Beweeg motor1 naar sensor");
  moveStepper(stepPin1, dirPin1, 100, true);
  delay(500);

  int ldrValue = analogRead(ldrPin);
  Serial.print("LDR waarde: ");
  Serial.println(ldrValue);

  // Drempelwaarden moeten kalibreren!
  if (ldrValue < 300) {
    Serial.println("Kleur gedetecteerd: Blauw");
    moveToContainer(stepsToContainerBlue);
  } else if (ldrValue < 600) {
    Serial.println("Kleur gedetecteerd: Rood");
    moveToContainer(stepsToContainerRed);
  } else {
    Serial.println("Kleur gedetecteerd: Groen");
    moveToContainer(stepsToContainerGreen);
  }

  delay(2000);

  Serial.println("Motor1 terug naar start");
  moveStepper(stepPin1, dirPin1, 100, false);
  delay(1000);

  Serial.println("Motor2 terug naar start");
  // Terug naar start door omgekeerd te draaien naar blauwe bakje
  moveToContainer(-stepsToContainerBlue);
  delay(1000);
}

void moveStepper(int stepPin, int dirPin, int steps, bool forward) {
  digitalWrite(dirPin, forward ? HIGH : LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(800);
  }
}

void moveToContainer(int steps) {
  bool forward = steps > 0;
  int absSteps = abs(steps);
  digitalWrite(dirPin2, forward ? HIGH : LOW);
  for (int i = 0; i < absSteps; i++) {
    digitalWrite(stepPin2, HIGH);
    delayMicroseconds(800);
    digitalWrite(stepPin2, LOW);
    delayMicroseconds(800);
  }
}