#include <Servo.h>
#include <Arduino_APDS9960.h>


int servoRPin = D12;
int servoLPin = D11;

int d = 100;

Servo servoR;
Servo servoL;

bool updatePosition = true;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servoR.attach(servoRPin);
  servoL.attach(servoLPin);
  
  if (!APDS.begin()) {
    Serial.println("Error initializing APDS-9960 sensor.");
  }

}

void loop() {
  // put your main code here, to run repeatedly:
  while (! APDS.colorAvailable()) {
    delay(5);
  }
  int r, g, b;

  // read the color
  APDS.readColor(r, g, b);

  Serial.print("r = ");
  Serial.println(r);
  Serial.print("g = ");
  Serial.println(g);
  Serial.print("b = ");
  Serial.println(b);
  Serial.println();

  servoR.write( 0 );
  servoL.write( 180 );

  if ((r > 255) || (g > 255) || (b > 255)) {
    delay(1000);
    

  } else if ((r > b) && (r > g)){  //RED
    servoR.write( 180 );
    servoL.write( 0  );
    delay(100);    
    servoR.write( 0 );
    servoL.write( 180  );
    delay(100);
    servoR.write( 180 );
    servoL.write( 0  );
    delay(100);
    servoR.write( 0 );
    servoL.write( 180  );
    delay(100);
  
  } else if ((b > r)){ //BLUE
    servoR.write( 180 );
    servoL.write( 180  );
    delay(200);
    servoR.write( 0 );
    servoL.write( 0  );
    delay(200);
    servoR.write( 180 );
    servoL.write( 180  );
    delay(200);

  } else { //GREEN
    servoR.write( 60 );
    delay(100);
    servoR.write( 120 );
    delay(100);
    servoR.write( 180 );
    delay(300);
  }
  
}