#include <Servo.h>

Servo myServo;
bool colorPresent;
int trig1 = 6; //trigger pin 1 connected to 6th pin of bluepill
int echo1 = 7; //echo pin 1 connected to 7th pin of bluepill
int trig2 = 8; //trigger pin 2 connected to 8th pin of bluepill
int echo2 = 9; // echo pin 2 connected to 9th pin of bluepill
int S0 = 31;
int S1 = 30;
int S2 = PIN_A4;
int S3 = PIN_A5;
int OUT = PIN_A0;
unsigned int red, green, blue;
bool blueDetected = false;
bool servoMoved = false;
unsigned long blueDetectedTime = 0;
unsigned long servoMoveStartTime = 0;
unsigned long lastColorSenseTime=0;
unsigned long colorSenseInterval=50;
const int colorPresenceThreshold = 200;
const unsigned long motorPauseDuration = 100;
bool motorPaused = false;
unsigned long motorPauseStartTime = 0;
const unsigned long delayAfterBlue = 350;      // Wait 2 seconds after blue is detected
const unsigned long servoHoldDuration = 100;
bool colorPauseActive = false;


int distance1; //distance of object
int distance2;
long timetaken1; //timetaken for ultrasonic sound wave to echo back
long timetaken2;
bool objectdetected1=false;
bool objectdetected2=false;
unsigned long lastScanTime1 = 0;
unsigned long lastScanTime2 = 0;
unsigned long lastScanTime3 = 0;
const int scanInterval1 = 320;
const int scanInterval2 = 30;




int IN1= PIN_A8; //Input 1 of motor driver
int IN2= PIN_A3; //input 2 of motor driver
int EN1= PIN_A7; //enable pin of motor driver
int Count=0;

int readColor(int S2_val, int S3_val) 
{
  digitalWrite(S2, S2_val);
  digitalWrite(S3, S3_val);
  delay(100); // small delay
  return pulseIn(OUT, LOW);
}

void setup() {
 
 //initialise UART
  Serial.begin(9600); 
  Serial1.begin(9600); 

  

  pinMode(trig1,OUTPUT); // sets the trigger pin 1 as output mode
  pinMode(echo1,INPUT); // sets the echo pin 1 as input mode
  pinMode(trig2,OUTPUT); // sets the trigger pin 2 as output mode
  pinMode(echo2,INPUT); // sets the trigger pin 2 as input mode

  pinMode(IN1,OUTPUT); //set input 1 to output mode
  pinMode(IN2,OUTPUT); //set input 2 to output mode
  pinMode(EN1,OUTPUT); //set enable pin to output mode

  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(OUT, INPUT);

  digitalWrite(S0, HIGH);
  digitalWrite(S1, LOW);

  myServo.attach(3);
  myServo.write(0);
  


}

void loop() {

  if (millis() - lastScanTime1 >= scanInterval1) 
  {
    lastScanTime1 = millis();

    // Ultrasonic scan
    digitalWrite(trig1, LOW);
    delayMicroseconds(2);
    digitalWrite(trig1, HIGH); 
    delayMicroseconds(10);
    digitalWrite(trig1, LOW);

    timetaken1 = pulseIn(echo1, HIGH, 10000);
    distance1 = timetaken1 * 0.034 / 2;

    if (distance1 > 0 && distance1 < 15) 
    {
        Count+=1; 
    }
  }


  if (millis() - lastScanTime2 >= scanInterval2) 
  {
    digitalWrite(trig2,LOW); // clearing the trigger pin 2

    delayMicroseconds (2);

    digitalWrite (trig2, HIGH); // sets the trigger pin to HIGH state for 10 µsec
  
    delayMicroseconds (10);

    digitalWrite (trig2, LOW);
  
    timetaken2 = pulseIn(echo2, HIGH, 10000);
    // calculates the time taken by pulse from echo pin

  
    distance2 = timetaken2 * 0.034/2;

    if (distance2 > 0 && distance2 < 15 && Count>0 )
    {
        Count-=1;
    }

    Serial1.print(Count);
    Serial1.println(" Object detected");
    Serial1.print(Count);
    Serial1.println(" Object Remaining");

    
  }

  
  
  

  if (millis() - lastColorSenseTime >= colorSenseInterval ) 
  {
    lastColorSenseTime = millis();

    // Read color values
    red = readColor(LOW, LOW);
    green = readColor(HIGH, HIGH);
    blue = readColor(LOW, HIGH);
    Serial.print("R: "); Serial.print(red);
    Serial.print(" G: "); Serial.print(green);
    Serial.print(" B: "); Serial.println(blue);
    colorPresent = (red < 190 || green < 190 || blue < 190);
    

    if (colorPresent && Count > 0 && motorPaused==false) 
    {

      // Stop motor
      digitalWrite(IN1, LOW);
      digitalWrite(IN2, LOW);
      analogWrite(EN1, 0);
      if (blue < red && blue < green && blue < 150 && Count>0 && !blueDetected)
      {
      
        Serial.println("Blue detected!");
        blueDetected=true;
        blueDetectedTime = millis();
      
      
      }
       
      motorPaused = true;
      motorPauseStartTime = millis();
    }

      // After pause, resume motor
    if (motorPaused==true && millis() - motorPauseStartTime >= motorPauseDuration) 
    {
      motorPaused = false;
    }
    
  }
  
  if (Count > 0 && motorPaused==false)
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(EN1, 150);
  } 
  if(Count==0)
  {
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    analogWrite(EN1, 0);
  }
  
  

  if (blueDetected && !servoMoved && (millis() - blueDetectedTime >= delayAfterBlue)) 
  {
    myServo.write(120);
    servoMoveStartTime = millis();
    servoMoved = true;
    Count -= 1;
  }


  if (servoMoved && (millis() - servoMoveStartTime >= servoHoldDuration)) 
  {
    myServo.write(0);
    servoMoved = false;
    blueDetected = false;
  }
  
 
}