#include <Servo.h>  // Inclui a biblioteca para controlar servos

Servo myServo;  // Cria um objeto servo
bool colorPresent;  // Indica se uma cor foi detectada
int trig1 = 6; // Pino trigger do sensor ultrassônico 1 conectado ao pino 6
int echo1 = 7; // Pino echo do sensor ultrassônico 1 conectado ao pino 7
int trig2 = 8; // Pino trigger do sensor ultrassônico 2 conectado ao pino 8
int echo2 = 9; // Pino echo do sensor ultrassônico 2 conectado ao pino 9
int S0 = 31; // Pino S0 do sensor de cor
int S1 = 30; // Pino S1 do sensor de cor
int S2 = PIN_A4; // Pino S2 do sensor de cor
int S3 = PIN_A5; // Pino S3 do sensor de cor
int OUT = PIN_A0; // Pino de saída de dados do sensor de cor
unsigned int red, green, blue; // Variáveis para armazenar os valores das cores
bool blueDetected = false; // Flag para indicar se azul foi detectado
bool servoMoved = false; // Flag para indicar se o servo já se moveu
unsigned long blueDetectedTime = 0; // Armazena o tempo de detecção da cor azul
unsigned long servoMoveStartTime = 0; // Armazena o tempo de início do movimento do servo
unsigned long lastColorSenseTime=0; // Armazena o último tempo em que a cor foi lida
unsigned long colorSenseInterval=50; // Intervalo entre leituras de cor
const int colorPresenceThreshold = 200; // Limiar de detecção de cor
const unsigned long motorPauseDuration = 100; // Tempo de pausa do motor após detecção de cor
bool motorPaused = false; // Flag que indica se o motor está pausado
unsigned long motorPauseStartTime = 0; // Tempo em que a pausa do motor começou
const unsigned long delayAfterBlue = 350; // Atraso após detectar azul para mover o servo
const unsigned long servoHoldDuration = 100; // Tempo que o servo permanece na posição ativada
bool colorPauseActive = false; // Flag auxiliar (não utilizada neste código)

int distance1; // Distância medida pelo sensor ultrassônico 1
int distance2; // Distância medida pelo sensor ultrassônico 2
long timetaken1; // Tempo da resposta do sensor 1
long timetaken2; // Tempo da resposta do sensor 2
bool objectdetected1=false; // Flag de detecção do sensor 1 (não usada diretamente)
bool objectdetected2=false; // Flag de detecção do sensor 2 (não usada diretamente)
unsigned long lastScanTime1 = 0; // Último tempo de varredura do sensor 1
unsigned long lastScanTime2 = 0; // Último tempo de varredura do sensor 2
unsigned long lastScanTime3 = 0; // Variável não utilizada
const int scanInterval1 = 320; // Intervalo de varredura do sensor 1
const int scanInterval2 = 30; // Intervalo de varredura do sensor 2

int IN1= PIN_A8; // Entrada 1 do driver de motor
int IN2= PIN_A3; // Entrada 2 do driver de motor
int EN1= PIN_A7; // Pino de habilitação do driver de motor
int Count=0; // Contador de objetos

int readColor(int S2_val, int S3_val) // Função para ler valores de cor
{
  digitalWrite(S2, S2_val); // Define o valor de S2
  digitalWrite(S3, S3_val); // Define o valor de S3
  delay(100); // Pequeno atraso
  return pulseIn(OUT, LOW); // Retorna o tempo do pulso LOW do pino OUT
}

void setup() {
  Serial.begin(9600); // Inicializa comunicação serial
  Serial1.begin(9600); // Inicializa segunda serial (para comunicação com outro dispositivo)

  pinMode(trig1,OUTPUT); // Configura trig1 como saída
  pinMode(echo1,INPUT); // Configura echo1 como entrada
  pinMode(trig2,OUTPUT); // Configura trig2 como saída
  pinMode(echo2,INPUT); // Configura echo2 como entrada

  pinMode(IN1,OUTPUT); // Configura IN1 como saída
  pinMode(IN2,OUTPUT); // Configura IN2 como saída
  pinMode(EN1,OUTPUT); // Configura EN1 como saída

  pinMode(S0, OUTPUT); // Configura pino S0 como saída
  pinMode(S1, OUTPUT); // Configura pino S1 como saída
  pinMode(S2, OUTPUT); // Configura pino S2 como saída
  pinMode(S3, OUTPUT); // Configura pino S3 como saída
  pinMode(OUT, INPUT); // Configura pino OUT como entrada

  digitalWrite(S0, HIGH); // Define S0 como HIGH (configuração do sensor)
  digitalWrite(S1, LOW); // Define S1 como LOW (configuração do sensor)

  myServo.attach(3); // Conecta o servo no pino 3
  myServo.write(0); // Inicializa o servo na posição 0
}

void loop() {

  if (millis() - lastScanTime1 >= scanInterval1) // Verifica se é hora de escanear o sensor 1
  {
    lastScanTime1 = millis(); // Atualiza o tempo da última varredura

    digitalWrite(trig1, LOW); // Garante nível baixo
    delayMicroseconds(2); // Atraso curto
    digitalWrite(trig1, HIGH); // Gera pulso de trigger
    delayMicroseconds(10);
    digitalWrite(trig1, LOW);

    timetaken1 = pulseIn(echo1, HIGH, 10000); // Mede tempo de resposta do echo
    distance1 = timetaken1 * 0.034 / 2; // Converte tempo em distância

    if (distance1 > 0 && distance1 < 15) // Se objeto detectado próximo
    {
        Count+=1; // Incrementa contador
    }
  }

  if (millis() - lastScanTime2 >= scanInterval2) // Verifica se é hora de escanear o sensor 2
  {
    digitalWrite(trig2,LOW); // Garante nível baixo
    delayMicroseconds (2);
    digitalWrite (trig2, HIGH); // Gera pulso de trigger
    delayMicroseconds (10);
    digitalWrite (trig2, LOW);

    timetaken2 = pulseIn(echo2, HIGH, 10000); // Mede tempo de resposta do echo
    distance2 = timetaken2 * 0.034/2; // Converte tempo em distância

    if (distance2 > 0 && distance2 < 15 && Count>0 ) // Se objeto detectado saindo
    {
        Count-=1; // Decrementa contador
    }

    Serial1.print(Count); // Envia número de objetos detectados
    Serial1.println(" Object detected");
    Serial1.print(Count);
    Serial1.println(" Object Remaining");
  }

  if (millis() - lastColorSenseTime >= colorSenseInterval ) // Verifica intervalo de leitura de cor
  {
    lastColorSenseTime = millis(); // Atualiza tempo

    red = readColor(LOW, LOW); // Lê vermelho
    green = readColor(HIGH, HIGH); // Lê verde
    blue = readColor(LOW, HIGH); // Lê azul
    Serial.print("R: "); Serial.print(red); // Mostra valores no monitor serial
    Serial.print(" G: "); Serial.print(green);
    Serial.print(" B: "); Serial.println(blue);
    colorPresent = (red < 190 || green < 190 || blue < 190); // Verifica se alguma cor está presente

    if (colorPresent && Count > 0 && motorPaused==false) // Se cor presente e objetos presentes
    {
      digitalWrite(IN1, LOW); // Para motor
      digitalWrite(IN2, LOW);
      analogWrite(EN1, 0);

      if (blue < red && blue < green && blue < 150 && Count>0 && !blueDetected) // Detecta azul
      {
        Serial.println("Blue detected!");
        blueDetected=true;
        blueDetectedTime = millis(); // Salva tempo de detecção
      }

      motorPaused = true; // Pausa motor
      motorPauseStartTime = millis(); // Salva tempo da pausa
    }

    if (motorPaused==true && millis() - motorPauseStartTime >= motorPauseDuration) // Após pausa
    {
      motorPaused = false; // Libera motor
    }
  }

  if (Count > 0 && motorPaused==false) // Se há objetos e motor pode girar
  {
    digitalWrite(IN1, LOW); // Define direção
    digitalWrite(IN2, HIGH);
    analogWrite(EN1, 150); // Liga motor com velocidade 150
  } 
  if(Count==0) // Se não há objetos
  {
    digitalWrite(IN1, LOW); // Para motor
    digitalWrite(IN2, LOW);
    analogWrite(EN1, 0);
  }

  if (blueDetected && !servoMoved && (millis() - blueDetectedTime >= delayAfterBlue)) // Após atraso do azul
  {
    myServo.write(120); // Move o servo
    servoMoveStartTime = millis(); // Marca início
    servoMoved = true;
    Count -= 1; // Atualiza contador
  }

  if (servoMoved && (millis() - servoMoveStartTime >= servoHoldDuration)) // Após tempo de segurar
  {
    myServo.write(0); // Retorna servo à posição inicial
    servoMoved = false; // Libera para outro movimento
    blueDetected = false; // Libera para outra detecção de azul
  }
}