// I have adhered to the Duke Community Standard in completing this assignment
// NetID: nrm27
#include <Servo.h>
#include <Ping.h>
#define Rx 17
#define Tx 16

// Instantiate sensor and servos
Ping ping = Ping(3,74,29);
Servo servoLeft;
Servo servoRight;

// Declare global variables
int red = 45;
int green = 46;
int blue = 44;
String paddles = "";
String quaffles = "";
int score = 0;
int qTotal = 0;
int halted = 0;
bool seeker= false;
bool p1 = false;
bool finished = false;

void setup() {
  Serial.begin(9600);
  servoLeft.attach(12);
  servoRight.attach(11);

  pinMode(red, OUTPUT);
  pinMode(green, OUTPUT);
  pinMode(blue, OUTPUT);
  digitalWrite(blue, HIGH);

  Serial.begin(9600);
  Serial2.begin(9600);
  Serial3.begin(9600);

  // Start up display
  delay(100);
  Serial3.write(17);
  delay(5);
  Serial3.write(12);
  delay(3000);
  Serial3.write(18);
}

void loop() {
  char outgoing = " ";
  boolean left = readSensor(53);
  boolean middle = readSensor(51);
  boolean right = readSensor(49);
  digitalWrite(red, HIGH);
  digitalWrite(green, HIGH);

  if (!left && middle && !right) { // Go straight, middle sensor is on
    setServos(1.2, 1.2);
    halted = 0;
    delay(50);
  }
  else if ((left && middle && !right) || (left && !middle && !right)) { // Turn left, middle/right sensors off
    setServos(0, 0.1);
    halted = 0;
    delay(75);
  }
  else if ((!left && middle && right)||(!left && !middle && right)) { // Turn right, middle/left sensors off
    setServos(0.1, 0);
    halted = 0;
    delay(75);
  }
  else if (left && right && middle && halted == 0) { // Reached a mark

    delay(100);
    setServos(0, 0);
    ping.fire();

    if(ping.inches() < 5.0) { // If quaffle detected, record that it was seen and flash green LED
      digitalWrite(green, LOW);
      quaffles += "s";
    }
    else { // If quaffle not detected, flash red and record that it wasn't seen
      digitalWrite(red, LOW);
      quaffles += "q";
    }

    delay(250);
    digitalWrite(red, HIGH);
    digitalWrite(green, HIGH);
    halted = 1;
    setServos(1,1);
    qTotal++; // Keep track of how many marks have been seen (qTotal = total quaffles seen)
    delay(200);


    if(qTotal == 5) { // Back up once 5 marks have been seen to allow seeker to drive by
      setServos(-0.3, -0.3);
        delay(800);
        setServos(0, 0);
    }
    while(qTotal == 5) { // Reached end of line, wait for partner information
      digitalWrite(blue, LOW);
      setServos(0, 0);

      if(Serial2.available()) { // If partner has sent characters, record characters in order
        char incoming = Serial2.read();
        if(incoming == 'r' || incoming == 'g') {
          paddles += incoming;
        }
        delay(50);
      }

      if(paddles.length() >= 5) { // Enter conditional if all data from partner has been received
        Serial.println("received data");
        digitalWrite(red, HIGH);
        delay(1000);
        digitalWrite(red, LOW);
        delay(1500);

        for(int i = 0; i < paddles.length(); i++) {
          if(quaffles.charAt(i) == 's' && paddles.charAt(i) == 'r') {
            score += 10; // Add 10 points each time there is a matching quaffle and paddle
          }
          char outgoing = quaffles.charAt(i);
          Serial2.print(outgoing);
          delay(300);
        }
        digitalWrite(blue, HIGH);
        digitalWrite(red, HIGH);

        // Determine score to send other 2 teams
        char outgoing = 'w';
        if(score == 0)
          outgoing = 'f';
        if(score == 10)
          outgoing = 'a';
        if(score == 20)
          outgoing = 'b';
        if(score == 30)
          outgoing = 'c';
        if(score == 40)
          outgoing = 'd';
        if(score == 50)
          outgoing = 'e';

        delay(100);
        Serial.print("Score before ");
        Serial.println(score);

        while (!finished) { // Wait for data from other 2 teams to calculate final score
          Serial2.print(outgoing);
          delay(100);

          if (Serial2.available()) {
            char incoming = Serial2.read();
            delay(50);
            Serial.print("p1: ");
            Serial.print(p1);
            Serial.print(" seeker: ");
            Serial.println(seeker);

            if (!p1) { // Received data from other pair
              if (incoming == 'A') {
                score += 10;
                p1 = true;
              }
              else if (incoming == 'B') {
                score += 20;
                p1 = true;
              }
              else if (incoming == 'C') {
                score += 30;
                p1 = true;
              }
              else if (incoming == 'D') {
                score += 40;
                p1 = true;
              }
              else if (incoming == 'E') {
                score += 50;
                 p1 = true;
              }
              else if (incoming == 'F') {
                score += 0;
                p1 = true;
              }
            } // Received data from seeker
            if (!seeker) {
              if (incoming == 'x') {
                score += 0;
                seeker = true;
              }
              else if (incoming == 'z') {
                score += 150;
                seeker = true;
              }
            }

            if (p1 && seeker) { // Once seeker and p1 have been seen, exit
              finished = true;
            }
          }
        }

        Serial.print("Score after ");
        Serial.println(score);



        while(true) { // Entire routine is finished, display final score and blink green
          Serial2.print(outgoing);
          digitalWrite(green, LOW);
          delay(250);
          digitalWrite(green, HIGH);
          delay(250);
          Serial3.write(12);
          Serial3.write(17);
          delay(5);
          Serial3.print("Total Points:");
          Serial3.write(13);
          Serial3.print(score);
          delay(50);
        }
      }
    }
  }
}

boolean readSensor(int pin) { // Helper method to determine if QTI sensor sees black
  pinMode(pin, OUTPUT);
  digitalWrite(pin, HIGH);
  delayMicroseconds(230);
  pinMode(pin, INPUT);
  digitalWrite(pin, LOW);
  long time  = micros();
  while (digitalRead(pin));
  time = micros() - time;
  return time > 250;
}

void setServos(double left, double right) { // Helper method which makes setting servos simpler
  servoLeft.writeMicroseconds(1500 - (left * 200));
  servoRight.writeMicroseconds(1500 + (right * 200));
}