#include <AccelStepper.h>
#include <ESP32Servo.h>

const int s0 = 19;
const int s1 = 18;
const int out = 15;
const int s2 = 2;
const int s3 = 4;

int red, blue, green;
String lastColor = ""; // To keep track of the last detected color

// Define constants for servo control pin and angles
const int servoPin = 27;


// Create a Servo object for the servo motor
Servo myServo;

// Define pin constants for the second stepper motor
const int stepPin2 = 32;
const int dirPin2 = 33;

// Create an instance of the AccelStepper class for the second stepper motor
AccelStepper stepper2(AccelStepper::DRIVER, stepPin2, dirPin2);

void setup() {
  Serial.begin(9600);
  pinMode(s0, OUTPUT);
  pinMode(s1, OUTPUT);
  pinMode(s2, OUTPUT);
  pinMode(s3, OUTPUT);
  pinMode(out, INPUT);
  digitalWrite(s0, HIGH);
  digitalWrite(s1, HIGH);

  // Attach the servo to the defined pin
  myServo.attach(servoPin);

  // Set initial position of the servo motor


  // Initialize the second stepper motor
  stepper2.setMaxSpeed(250); // Adjust speed for the second stepper motor
  stepper2.setAcceleration(500); // Adjust acceleration for the second stepper motor
  stepper2.setSpeed(250); // Set initial speed for continuous rotation
}

void loop() {
  static unsigned long lastColorCheck = 0;
  if (millis() - lastColorCheck >= 500) {
    lastColorCheck = millis();
    color();
  }

  // Keep the second stepper motor spinning
  stepper2.runSpeed();
}

void color() {
  // Red
  digitalWrite(s2, LOW);
  digitalWrite(s3, LOW);
  delay(50);
  red = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);

  // Blue
  digitalWrite(s2, LOW);
  digitalWrite(s3, HIGH);
  delay(50);
  blue = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);

  // Green
  digitalWrite(s2, HIGH);
  digitalWrite(s3, HIGH);
  delay(50);
  green = pulseIn(out, digitalRead(out) == HIGH ? LOW : HIGH);

  // Print color values for debugging
  Serial.print("Rdeča: ");
  Serial.print(red);
  Serial.print(" Modra: ");
  Serial.print(blue);
  Serial.print(" Zelena: ");
  Serial.println(green);

  // Color recognition and servo motor movement
  if (red < blue && red < green && red < 75) {
    Serial.println("Barva: Rdeča");
    lastColor = "Rdeča";
    // Move servo to red position (240 degrees)
    myServo.write(180);
  } else if (blue < red && blue < green && blue < 75) {
    Serial.println("Barva: Modra");
    lastColor = "Modra";
    // Move servo to blue position (120 degrees)
    myServo.write(0);
  } else if (green < red && green < blue && green < 75) {
    Serial.println("Barva: Zelena");
    lastColor = "Zelena";
    // Move servo to green position (0 degrees)
    myServo.write(90);
  } else if (red > 90 && blue > 90 && green > 90) {
    Serial.println("Barva: Ni definirana");
  } else {
    Serial.println("Barva: Ni definirana");
  }
}