//We have adhered to the Duke Community Standard in Completing this Assignment
//Signed: Cameron Jarnot, Sophie Rubin

long rcTime(int pin, boolean print); //QTI values
#include <Servo.h> //servos
#include <Wire.h>
#include "Adafruit_TCS34725.h" //RGB
#define Rx 17 //Receiving
#define Tx 16 //Transmitting

const int TxPin = 10; //transmit pin

#include <SoftwareSerial.h> //include communication
SoftwareSerial mySerial = SoftwareSerial(255, TxPin); //set communication pins

int paddles = 0; //to count how far along the path it is
String colors = ""; //to record the paddle colors
char myScore = 'f'; //set score to zero
Adafruit_TCS34725 tcs = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X); //initialize RGB

Servo servoLeft; //turn on left servo
Servo servoRight; //turn on right servo

boolean seeker = false; //waiting for seeker score
boolean p1 = false; //waiting for other pair's score
boolean finished = false; //waiting to have all scores
boolean happening = false; //tracking whether or not it has score with chaser

void setup() {

  pinMode(7, OUTPUT); //non-built in LED
  pinMode(44, OUTPUT); //red
  pinMode(46, OUTPUT); //green
  pinMode(49,OUTPUT); //left QTI
  pinMode(51,OUTPUT); //center QTI
  pinMode(53,OUTPUT); //right QTI
  Serial.begin(9600);
  delay(300);
  servoLeft.attach(12); //connect to left servo
  servoRight.attach(11); //connect to right servo

  pinMode(LED_BUILTIN, OUTPUT);
  Serial.begin(9600);
  Serial2.begin(9600);
  delay(500);

  Serial3.begin(9600);
  delay(100);
  Serial3.write(17);                 // Turn backlight on
  delay(5);                           // Required delay
  Serial3.write(18);                 // Turn backlight off

}

void loop() {

    digitalWrite(44, LOW); //red on
    digitalWrite(46, LOW); //green on

    long valueL = rcTime(49, false); //left QTI
    long valueM = rcTime(51, false); //center QTI
    long valueR = rcTime(53, false); //right QTI

    boolean qtL = true; //left QTI sense (true for black)
    boolean qtM = true; //middle QTI sense (true for black)
    boolean qtR = true; //right QTI sense (true for black)
    if(valueL<300){ //if left rcTime less than threshold
      qtL = false; //it sees white
    }
    if(valueM<300){ //if center rcTime less than threshold
      qtM = false; //it sees white
    }
    if(valueR<300){ //if right rcTime less than threshold
      qtR = false; //it sees white
    }

     if(!qtL && qtM && !qtR){ //if only middle black
      servoLeft.writeMicroseconds(1540); //move forward
      servoRight.writeMicroseconds(1460);
      delay(5);
    }
    else if(qtR && !qtL){ //if left white and right black
      servoLeft.writeMicroseconds(1700); //turn right
      servoRight.writeMicroseconds(1500);
      delay(5);
    }
    else if(!qtR && qtL){ //if right white and left black
      servoLeft.writeMicroseconds(1500); //turn left
      servoRight.writeMicroseconds(1300);
      delay(5);
    }

    else if(qtL && qtM && qtR){ //if all black
      servoLeft.writeMicroseconds(1500); //stall, no movement
      servoRight.writeMicroseconds(1500);
      paddles+=1; //track one more hash
//      Serial.println("paddles: ");
//      Serial.println(paddles);
      uint16_t clear, red, green, blue; //see color from RGB
      delay(400);
      tcs.getRawData(&red, &green, &blue, &clear); //collect RGB data

      if(red>=green){ //if it is more red than green
        char outgoing = 'r'; //add red character to string
//        Serial.println("outgoing: ");
//        Serial.println(outgoing);
        colors += "r"; //track that paddle was red
        //Serial2.println('r');
        digitalWrite(46, HIGH); //blink darker blue
        delay(500);
        digitalWrite(46, LOW);
        delay(500); //wait .5sec
      }
      else if(green>red){ //if it is more green than red
        char outgoing = 'g'; //add green character to string
//        Serial.println("outgoing: ");
//        Serial.println(outgoing);
        colors += "g"; //track that paddle was green
        //Serial2.println('g');
        digitalWrite(44, HIGH); //blink green
        delay(500);
        digitalWrite(44, LOW);
        delay(500); //wait .5sec
      }

      servoLeft.writeMicroseconds(1540); //move forward
      servoRight.writeMicroseconds(1460);
      delay(400); //delay .2sec
  }

  if(paddles == 5) { //if it has sensed all five paddles

    digitalWrite(46, HIGH); //turn off built in LED
    digitalWrite(44, HIGH);

    servoLeft.writeMicroseconds(1500-(0.3*200)); //back up at the end
    servoRight.writeMicroseconds(1500+(0.3*200));
    delay(1200);
    servoLeft.writeMicroseconds(1500); //stop
    servoRight.writeMicroseconds(1500);
    digitalWrite(7, HIGH);
    //Serial3.print(colors);
    delay(3000);

    for(int i=0; i<5; i++){ //for each character in colors
      char outgoing = colors.charAt(i); //next letter of colors
      Serial2.print(outgoing); //send char
      //Serial3.print(outgoing); //print char to LCD
      //Serial3.write(12); //clear LCD
      delay(500); //wait .5sec
    }

    String chaser = ""; //create string for chaser's letters
    int score = 0; //initialize score
    while(!happening){
      if(Serial2.available()){ //if another robot is sending
        char in = Serial2.read(); //receive letter
        if(in == 's' || in == 'q'){ //if it's the chaser
          chaser += in; //add the letter to chaser string
          //Serial3.print(in); //print letter to LCD
        }
//        Serial.println(chaser);
      }
      if(chaser.length() == 5){ //once all 5 have been received
        delay(50);
        for(int i=0; i<5; i++){ //for each letter in colors and chaser
          if(colors.charAt(i) == 'r' && chaser.charAt(i) == 's'){ //if quaffle and red
            score += 10; //add ten to score
          }
        }
        //Serial3.write(12); //clear LCD
        //Serial3.print("Score: ");
        //Serial3.print(score); //print score
        happening = true; //has score with chaser
      }

    }

    while(!finished){ //while it doesn't have the total
      delay(1);
      if(Serial2.available()){ //if someone is sending
        char seeIn = Serial2.read(); //read the character

        if(! p1){ //if p1 score hasn't been received
          if(seeIn == 'A'){ //if 'A' is sent
            score += 10; //add ten to score
            p1 = true; //has received p1
          }
          if(seeIn == 'B'){ //if 'B' is sent
            score += 20; //add twenty to score
            p1 = true; //has received p1
          }
          if(seeIn == 'C'){ //if 'C' is sent
            score += 30; //add thirty to score
            p1 = true; //has received p1
          }
          if(seeIn == 'D'){ //if 'D' is sent
            score += 40; //add forty to score
            p1 = true; //has received p1
          }
          if(seeIn == 'E'){ //if 'E' is sent
            score += 50; //add fifty to score
            p1 = true; //has received p1
          }
          if(seeIn == 'F'){ //if 'F' is sent
            p1 = true; //do not add to score, has received p1
          }
        }

        if(! seeker){ //if seeker score hasn't been received
          if(seeIn == 'x'){ //if 'x' is received
            seeker = true; //do not add to score, has received seeker
          }
          if(seeIn == 'z'){ //if 'z' is received
            score += 150; //add 150 to score
            seeker = true; //has received seeker
          }
        }

        delay(1);
        if(p1 && seeker){ //if it has all bots' scores
          Serial3.write(12); //clear screen
          delay(5);
          Serial3.print("Total score: ");
          Serial3.println(score); //print total score
          Serial3.print(colors); //print paddles' colors seen
          finished = true; //consider computation complete
        }
      }

      while(finished){ //while computation is complete
        Serial3.write(12); //clear screen
          Serial3.print("Total score: "); //print total score
          Serial3.println(score); //print total score
          Serial3.print(colors); //print paddles' colors seen
          delay(1000);

      }
    }

  }

}

long rcTime(int pin, boolean print) { //calculate QTI values
 pinMode(pin, OUTPUT); //send IR signal
 digitalWrite(pin, HIGH);
 delayMicroseconds(230);
 pinMode(pin, INPUT); //receive IR signal
 digitalWrite(pin, LOW); long
 time = micros(); while
 (digitalRead(pin)); time =
 micros() - time; //calculate time constant
 return time;
}