/* ===============
   Project #3!
  ================= */

//#include <Adafruit_NeoPixel.h>
#include <Servo.h>
#include <Wire.h>
#include "Adafruit_TCS34725.h"

#define NEW_PIXEL_PIN 6
#define NUMPIXELS 4

#define servoPin1 2
#define servoPin2 3

/* Connect SCL    to A5
   Connect SDA    to A4
*/

//Adafruit_NeoPixel pixels(NUMPIXELS, NEW_PIXEL_PIN, NEO_GRB + NEO_KHZ800);
Adafruit_TCS34725 tcs = Adafruit_TCS34725();
Servo Servo1, Servo2;

uint16_t r, g, b, c, colorTemp, lux;

void setup() {
  Serial.begin(9600);

  Serial.println("Starting!");
//  pixels.begin();
  Servo1.attach(servoPin1);
  Servo2.attach(servoPin2);

  Serial.println(tcs.begin());
  if (tcs.begin()) {
    Serial.println("Found sensor");
  } else {
    Serial.println("No TCS34725 found ... check your connections");
    while (1);
  }
}

void loop() {
  get_color();

  if (r > 0) {
    servo(Servo1, 3);
  } else if (b > 0) {
    servo(Servo2, 3);
  } else { // green
    servo(Servo1, 3);
    servo(Servo2, 3);
  }
  delay(500);
}

void servo(Servo servo, int times) {
  for (int i = 0; i < times; i++) {
    servo.write(i * 90);
    delay(500); //#1 sec delay
  }
  for (int i = times; i > 0; i--) {
    servo.write(i * 90);
    delay(500); //#1 sec delay
  }
}

//void neo_pixel() {
//  pixels.clear();
//
//  for (int i = 0; i < NUMPIXELS; i++) {
//
//    pixels.setPixelColor(i, pixels.Color(0, 150, 0));
//    pixels.show();
//    delay(500);
//
//  }
//}

void get_color() {
  tcs.getRawData(&r, &g, &b, &c);
  colorTemp = tcs.calculateColorTemperature_dn40(r, g, b, c);

  Serial.print("R: "); Serial.print(r, DEC); Serial.print(" ");
  Serial.print("G: "); Serial.print(g, DEC); Serial.print(" ");
  Serial.print("B: "); Serial.print(b, DEC); Serial.print(" ");
  Serial.println(" ");
}