#include <Servo.h>
#include <NewPing.h>
#include <tcs3200.h>

Servo data1 ;
Servo data2 ;

int E;
int C;

const int echo2 = 15;
const int trig2 = 14;

const int D_MAX2 = 1000;

int D_SIN2;
float D_PRO2;

int D_LIS2[5] = {0, 0, 0, 0, 0}; 
int index2 = 0;

NewPing sonar1(trig2, echo2, D_MAX2);

int R_SIN;   
int G_SIN; 
int B_SIN;   
int L_BIT;  

float R_NOR; 
float G_NOR; 
float B_NOR;

tcs3200 tcs(5, 6, 7, 8, 9); 

unsigned long M_PRE = 0; 
unsigned long M_ACT = 0; 
const unsigned long M_INT = 500;  

char comando;

int boton1 = 3;
int boton2 = 4;
int buttom1;
int buttom2;

void setup() {
  // put your setup code here, to run once:
 Serial1.begin(9600);

 data1.attach(16);
 data2.attach(10);

 pinMode(trig2, OUTPUT);
 pinMode(echo2,INPUT);

  data1.write(0);
  data2.write(0);

  pinMode(7, INPUT_PULLUP);
  pinMode(6, INPUT_PULLUP);
}

void loop() {
  // put your main code here, to run repeatedly:

 M_ACT = millis();

 if (M_ACT - M_PRE >= M_INT) {
 
   M_PRE = M_ACT;

 buttom1 = digitalRead(3);
 buttom2 = digitalRead(4);

 if(buttom1 == 0 & buttom2 == 0){
  C = 1;
  } 
else if (buttom1 == 1 & buttom2 ==1){
  C = 0;
 } else {
  C = 2;
 }

 D_SIN2 = sonar1.ping_cm();

 D_LIS2[index2] = D_SIN2;
  index2 = (index2 + 1) % 5;
  D_PRO2 = (D_LIS2[0] + D_LIS2[1] + D_LIS2[2]+ D_LIS2[3] + D_LIS2[4]) / 5;

  R_SIN = tcs.colorRead('r', 20);   
  G_SIN = tcs.colorRead('g', 20);    
  B_SIN = tcs.colorRead('b', 20);    
  L_BIT = tcs.colorRead('c', 100);   
  
  if (L_BIT != 0) { 
    R_NOR = (float)R_SIN / L_BIT * 255;
    G_NOR = (float)G_SIN / L_BIT * 255;
    B_NOR = (float)B_SIN / L_BIT * 255;
  } else { 
    G_NOR = 0;
    B_NOR = 0;
  }

    if (R_NOR > G_NOR && R_NOR > B_NOR && L_BIT >= 80) {
    E = 2;
  }
  if (G_NOR > R_NOR && G_NOR > B_NOR && L_BIT >= 80) { 
    E = 1;
      }
  if (B_NOR > G_NOR && B_NOR > R_NOR && L_BIT >= 80) { 
   E = 0; 
  }
  if (L_BIT <= 80) {  
  E = 3;
  }
}

Serial1.println(String(D_PRO2) + "D" + String(E) + "E" + String(C) + "C" + "C\n");

  comando = Serial1.read();

 switch (comando){
  case'Q':
     data1.write(0);
     data2.write(0);
     delay(500);
     data1.write(180);  
     data2.write(180); 
     delay(500);
     break;

  case'q':
     data1.write(90);  
     data2.write(90); 
     break;
  case'E':
     data1.write(180);
     data2.write(180);
     delay(500);
     data1.write(0);  
     data2.write(0); 
     delay(500);
     break;
  case'e':
     data1.write(90);  
     data2.write(90);
     break;
}
}