#include <Arduino.h>
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_TCS34725.h>
const uint8_t ENA=5, ENB=6, IN1=7, IN2=8, IN3=9, IN4=10, CLAW=2;
const uint8_t IR[3]={A0,A1,A2};
// Tune these with raw readings printed while stopped. Flip polarity if needed.
const int LINE_THRESHOLD=500;
const bool BLACK_IS_HIGH=true;
const int OPEN_ANGLE=70, CLOSED_ANGLE=110;
Servo claw;
Adafruit_TCS34725 color(TCS34725_INTEGRATIONTIME_50MS,TCS34725_GAIN_4X);
bool sensorOK=false, running=false, handled=false;
uint8_t stable=0, lastColor=0;
unsigned long lastSample=0;
void motors(int left,int right){
 digitalWrite(IN1,left>0); digitalWrite(IN2,LOW);
 digitalWrite(IN3,right>0); digitalWrite(IN4,LOW);
 analogWrite(ENA,constrain(left,0,180)); analogWrite(ENB,constrain(right,0,180));
}
uint8_t classify(uint16_t r,uint16_t g,uint16_t b,uint16_t c){
 if(c<50 || c>60000) return 0;
 float sum=(float)r+g+b;
 if(sum<1) return 0;
 float R=r/sum,G=g/sum,B=b/sum;
 if(R>0.36 && G>0.33 && B<0.22) return 4;
 if(R>0.46 && R>G*1.3 && R>B*1.3) return 1;
 if(G>0.42 && G>R*1.2 && G>B*1.2) return 2;
 if(B>0.40 && B>R*1.2 && B>G*1.15) return 3;
 return 0;
}
void setup(){
 const uint8_t outputs[]={ENA,ENB,IN1,IN2,IN3,IN4};
 for(uint8_t p: outputs) {pinMode(p,OUTPUT);digitalWrite(p,LOW);}
 Serial.begin(115200);
 claw.write(OPEN_ANGLE); claw.attach(CLAW);
 Wire.begin(); Wire.setWireTimeout(25000,true);
 sensorOK=color.begin();
 Serial.println(F("Stopped. f=follow, s=stop, o=open/drop, c=close/pick. Reset to rearm color pickup."));
 if(!sensorOK) Serial.println(F("Color sensor missing: motion locked."));
}
void loop(){
 if(Serial.available()){
  char cmd=Serial.read();
  if(cmd=='s'){running=false; motors(0,0);}
  if(cmd=='f' && sensorOK){running=true;}
  if(cmd=='o' || cmd=='c'){running=false;motors(0,0);claw.write(cmd=='o'?OPEN_ANGLE:CLOSED_ANGLE);}
 }
 if(millis()-lastSample<80) return;
 lastSample=millis();
 int raw[3];bool line[3];
 for(uint8_t i=0;i<3;i++){raw[i]=analogRead(IR[i]); line[i]=BLACK_IS_HIGH?raw[i]>LINE_THRESHOLD:raw[i]<LINE_THRESHOLD;}
 uint16_t r=0,g=0,b=0,c=0;
 if(sensorOK) color.getRawData(&r,&g,&b,&c);
 uint8_t detected=classify(r,g,b,c);
 stable=detected && detected==lastColor?min(10,(int)stable+1):0;
 lastColor=detected;
 if(running && !handled && stable>=3){
  motors(0,0);running=false;handled=true;claw.write(CLOSED_ANGLE);
  Serial.println(F("Colored marker: stopped and closed claw. o=drop; f=continue."));
 }
 if(running){
  if(!line[0]&&!line[1]&&!line[2]){motors(0,0);running=false;Serial.println(F("Line lost: stopped."));}
  else if(line[0]&&line[2]){motors(0,0);running=false;Serial.println(F("Junction: stopped."));}
  else if(line[0])motors(70,140);
  else if(line[2])motors(140,70);
  else motors(120,120);
 }else motors(0,0);
 static unsigned long report=0;
 if(millis()-report>=500){report=millis();
  Serial.print(F("IR L/C/R: "));for(int v:raw){Serial.print(v);Serial.print(' ');}
  Serial.print(F(" RGB/C: ")); Serial.print(r);Serial.print(' ');Serial.print(g);Serial.print(' ');Serial.print(b);Serial.print(' ');Serial.print(c);
  Serial.print(F(" color: "));
  switch(detected){case 1:Serial.println(F("RED"));break;case 2:Serial.println(F("GREEN"));break;case 3:Serial.println(F("BLUE"));break;case 4:Serial.println(F("YELLOW"));break;default:Serial.println(F("unknown"));}
 }
}