e//Need to install Adafruit SSD1331, Adafruit GFX, PubSubClient Library


//Line Following Related
///////////////////////////////////////////////////////////////////////////
#include <Servo.h>
#define MaxSpeed 2000// max speed of the robot
#define BaseLeftSpeed 1850
#define BaseRightSpeed 1190
#define Kp 500 // experiment to determine this, start by something small that just makes your bot follow the line at a slow speed
#define Kd 40
#define echoPin 3

//OLED Related
///////////////////////////////////////////////////////////////////////////
#define BLACK           0x0000
#define BLUE            0x0006
#define RED             0xF800
#define GREEN           0x07E0
#define CYAN            0x07FF
#define MAGENTA         0xF81F
#define YELLOW          0xFFE0
#define WHITE           0xFFFF
#define BACKGROUND      0x0000

#include <Adafruit_SSD1331.h>

//colorPAL related
///////////////////////////////////////////////////////////////////////////
#include <SoftwareSerial.h>

const int sio = 51;      // ColorPAL connected to pin 51
const int unused = 255;     // Non-existant pin # for SoftwareSerial
const int sioBaud = 4800;
const int waitDelay = 200;

// Received RGB values from ColorPAL
int red;
int grn;
int blu;

// Set up two software serials on the same pin.
SoftwareSerial serin(sio, unused);
SoftwareSerial serout(unused, sio);

//Global
///////////////////////////////////////////////////////////////////////////
String mode = "DEFAULT";

//TO BE DONE
//const String DEFAULT_MODE = "D";
//const String LINE_FOLLOWING_MODE = "LF";
//const String LIGHT_TRACKING_MODE = "LT";

//D: Do nothing :)
//LF: Line FOllowing
//LT: Light Tracking


//MQTT Related
///////////////////////////////////////////////////////////////////////////
struct MqttMsg {
  String topic;
  String payload;
};


//Line Following Related
///////////////////////////////////////////////////////////////////////////
Servo left, right;
int l_qti, m_qti, r_qti; // left, middle, right

int lastError = 0;
unsigned long total_count = 0;
unsigned long count = 0;
bool arrivedTarget = false;    // Boolean variable for target coordinates equal to current coordinates


//Finding Red Spot Related
///////////////////////////////////////////////////////////////////////////
bool redSpotDetected = false;  // Boolean variable for red spot is found


//state set through network
int xTarget = 5;  //must be larger than 0
int yTarget = 5;
int xCurrent = 0;
int yCurrent = 0;
String carDirection = "N";  //NSEW
//end


double pos = 0; // car position

const int thre1 = 80; // qti threshold 1: white
const int thre2 = 300; // qti threshold 2
const int thre3 = 500; // qti threshold 3: black

//Light Tracking Related
///////////////////////////////////////////////////////////////////////////
double light = 0;

//OLED Related
///////////////////////////////////////////////////////////////////////////
byte previousByte = LOW;
byte currentByte = LOW;
int counter = 0;

int mosi = 31; //--- connect this to the display module DIN pin (Serial Data) //31
int sclk = 33; //--- connect this to the display module CLK pin (Serial Clock) //33
int cs   = 35; //--- connect this to the display module CS  pin (Chip Select) //35
int dc   = 37; //--- connect this to the display module D/C  pin (Data or Command) //37
int rst  = 39; //--- connect this to the display module RES pin (Reset)  //39

Adafruit_SSD1331 display = Adafruit_SSD1331(cs, dc, mosi, sclk, rst);

//GLOBAL VAR END
///////////////////////////////////////////////////////////////////////////



int get_qti(int pin) {
  digitalWrite(pin, HIGH);
  return analogRead(pin);
}

long get_ultrasonic_distance() {
  long duration, distance_in_cm;
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(echoPin, OUTPUT);
  digitalWrite(echoPin, LOW);
  delayMicroseconds(2);
  digitalWrite(echoPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(echoPin, LOW);
  pinMode(echoPin, OUTPUT);
  digitalWrite(echoPin, HIGH);
  duration = pulseIn(echoPin, HIGH);
  distance_in_cm = (duration / 2) / 29.1;
  return distance_in_cm;
}

void backward() {
  left.writeMicroseconds(1190);
  right.writeMicroseconds(1850);
}

void forward() {
  left.writeMicroseconds(1850);
  right.writeMicroseconds(1190);
}

void stay() {
  left.writeMicroseconds(1520);
  right.writeMicroseconds(1520);
}

void spin_left() {
  left.writeMicroseconds(1190);
  right.writeMicroseconds(1190);
}

void spin_right() {
  left.writeMicroseconds(1700);
  right.writeMicroseconds(1700);
}


void spin_right90() {
  // spin 90 degrees to right
  spin_right();
  delay(300);
  while (get_qti(A0) < thre3) {
    delay(2);
  }
  if (carDirection == "N")
    carDirection = "E";
  else if (carDirection == "E")
    carDirection = "S";
  else if (carDirection == "S")
    carDirection = "W";
  else if (carDirection == "W")
    carDirection = "N";
}

void spin_left90() {
  // spin 90 degrees to left
  spin_left();
  delay(300);
  while (get_qti(A2) < thre3) {
    delay(2);
  }
  if (carDirection == "N")
    carDirection = "W";
  else if (carDirection == "E")
    carDirection = "N";
  else if (carDirection == "S")
    carDirection = "E";
  else if (carDirection == "W")
    carDirection = "S";
}

void spin180() {
  spin_left();
  delay(300);
  while (get_qti(A2) < thre3) {
    delay(2);
  }
  spin_left();
  delay(300);
  while (get_qti(A2) < thre3) {
    delay(2);
  }

  if (carDirection == "N")
    carDirection = "S";
  else if (carDirection == "E")
    carDirection = "W";
  else if (carDirection == "S")
    carDirection = "N";
  else if (carDirection == "W")
    carDirection = "E";
}

bool getpos(int l_qti, int m_qti, int r_qti, double &res) {
  // return True if it detects the black line
  // return False if it loses track of the black line
  // res: result of predicting the position of the line
  // -1 < res < 1, which is the position of the black line
  // -ve res -> black line on the right
  // +ve res -> black line on the left

  if (m_qti < thre1 && r_qti < thre1 && l_qti > thre2) {
    // the line is closest to the left QTI sensor
    // map the value of l_qti from 0 ~ 1023 to -1 ~ -0.5
    res = l_qti / 1023.0 / 2.0 - 1;
    return true;
  }
  if (m_qti < thre1 && l_qti < thre1 && r_qti > thre2) {
    // the line is closest to the right QTI sensor
    // map the value from 0 ~ 1023 to 0.5 ~ 1
    res = 1 - r_qti / 1023.0 / 2.0;
    return true;
  }
  if (l_qti < thre1 && m_qti < thre1 && r_qti < thre1) {
    // All QTI sensors cannot find the line
    return false;
  }

  // Otherwise, the line is between the left and right QTI sensors
  res = (double)(r_qti - l_qti) / (l_qti + m_qti + r_qti);
  return true;
}

void linefollow(double error) {
  // difference in left/right wheel speed is proportional to
  // the deviation of the line position


  int motorSpeed = Kp * error + Kd * (error - lastError);
  lastError = error;

  int rightMotorSpeed = BaseRightSpeed + motorSpeed;
  int leftMotorSpeed = BaseLeftSpeed + motorSpeed;

  if (rightMotorSpeed > MaxSpeed ) rightMotorSpeed = MaxSpeed; // prevent the motor from going beyond max speed
  if (leftMotorSpeed > MaxSpeed ) leftMotorSpeed = MaxSpeed; // prevent the motor from going beyond max speed
  if (rightMotorSpeed < 0)rightMotorSpeed = 1516;
  if (leftMotorSpeed < 0)leftMotorSpeed = 1516;

  left.writeMicroseconds(leftMotorSpeed);
  right.writeMicroseconds(rightMotorSpeed);

}

//Setup
///////////////////////////////////////////////////////////////////////////
void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);

  //delay(2000); // Change to mqtt control
  pinMode(2, OUTPUT); //debug light
  pinMode(9, OUTPUT); // left servo
  pinMode(10, OUTPUT); // right servo
  pinMode(A0, INPUT); // right QTI
  pinMode(A1, INPUT); // middle QTI
  pinMode(A2, INPUT); // left QTI
  pinMode(echoPin, OUTPUT);
  left.attach(9);
  right.attach(10);

  stay();

  //OLED
  display.begin();
  display.fillScreen(BLACK);
  initOLED();

  //ColorPAL
  reset();          // Send reset to ColorPal
  serout.begin(sioBaud);
  pinMode(sio, OUTPUT);
  serout.print("= (00 $ m) !"); // Loop print values, see ColorPAL documentation
  serout.end();       // Discontinue serial port for transmitting

  serin.begin(sioBaud);         // Set up serial port for receiving
  pinMode(sio, INPUT);

}


//Line Following Method
///////////////////////////////////////////////////////////////////////////
long getUltrasonicDistance() {
  long duration, distance_in_cm;
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(echoPin, OUTPUT);
  digitalWrite(echoPin, LOW);
  delayMicroseconds(2);
  digitalWrite(echoPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(echoPin, LOW);
  pinMode(echoPin, INPUT);
  digitalWrite(echoPin, HIGH);
  duration = pulseIn(echoPin, HIGH);
  distance_in_cm = (duration / 2) / 29.1;
  return distance_in_cm;

}

void lineFollowing()
{
  r_qti = get_qti(A0);
  m_qti = get_qti(A1);
  l_qti = get_qti(A2);

  getpos(l_qti, m_qti, r_qti, pos);

  //sendToMQTT(String("ECHODATA"),String(analogRead(echoPin)));
  //Serial.print("PUB|ECHODATA:");

  if (xCurrent == xTarget && yCurrent == yTarget) {
    stay();
    arrivedTarget = true;

  } else {
    // count how many interceptions have been passed
    if (l_qti > thre3 && m_qti > thre3 && r_qti > thre3) {

      count++;
    } else {
      total_count = count;
      count = 0;
    }

    if (count > 10) { //intersection detected
      total_count = 0;
      count = 0;
      forward();
      delay(250);

      Serial.println("Intersection Detected");
      Serial.println(carDirection);
      Serial.println(xCurrent);
      Serial.println(yCurrent);
      Serial.println("");



      if (carDirection == "N") {
        yCurrent++;
      } else if (carDirection == "S") {
        yCurrent--;
      } else if (carDirection == "E") {
        xCurrent++;
      } else if (carDirection == "W") {
        xCurrent--;
      }
      if (xCurrent == xTarget && yCurrent == yTarget) {
        if (xCurrent == 0 && yCurrent == 0 && carDirection == "S" || xCurrent == 0 && yCurrent == 5 && carDirection == "W" || xCurrent == 5 && yCurrent == 0 && carDirection == "E" || xCurrent == 5 && yCurrent == 5 && carDirection == "N") {
          spin_left90();
        } else if (xCurrent == 0 && yCurrent == 0 && carDirection == "W" || xCurrent == 0 && yCurrent == 5 && carDirection == "N" || xCurrent == 5 && yCurrent == 0 && carDirection == "S" || xCurrent == 5 && yCurrent == 5 && carDirection == "E") {
          spin_right90();
        } else if ( xCurrent == 0 && carDirection == "W" || xCurrent == 5 && carDirection == "E" || yCurrent == 0 && carDirection == "S" || yCurrent == 5 && carDirection == "N") {
          spin180();
        }

      }


      if (xCurrent > xTarget) {
        if (yCurrent > yTarget) {
          if (carDirection == "N") {
            spin_left90();
          } else if (carDirection == "E") {
            spin_right90();
          } else if (carDirection == "S") {

          } else if (carDirection == "W") {

          }
        } else if (yCurrent == yTarget) {
          if (carDirection == "E") {
            //spin180();
          } else if (carDirection == "N") {
            spin_left90();
          } else if (carDirection == "S") {
            spin_right90();
          } else if (carDirection == "W") {

          }
        } else if (yCurrent < yTarget) {
          if (carDirection == "E") {
            spin_left90();
          } else if (carDirection == "S") {
            //spin180();
            spin_right90();
          } else if (carDirection == "N") {

          } else if (carDirection == "W") {
            //spin_right90();
          }
        }
      } else if (xCurrent == xTarget) {
        if (yCurrent > yTarget) {
          if (carDirection == "E") {
            spin_right90();
          } else if (carDirection == "W") {
            spin_left90();
          } else if (carDirection == "S") {

          } else if (carDirection == "N") {
            //spin180();
          }
        } else if (yCurrent < yTarget) {
          if (carDirection == "E") {
            spin_left90();
          } else if (carDirection == "W") {
            spin_right90();
          } else if (carDirection == "S") {
            //spin180();
          } else if (carDirection == "N") {

          }
        }

      } else if (xCurrent < xTarget) {
        if (yCurrent > yTarget) {
          if (carDirection == "W") {
            //spin180();
            spin_left90();
          }
          else if (carDirection == "N") {
            spin_right90();
          }
          else if (carDirection == "S") {
            //spin_left90();
          }
          else if (carDirection == "E") {

          }
        }
        else if (yCurrent == yTarget) {
          if (carDirection == "S") {
            spin_left90();
          }
          else if (carDirection == "N") {
            spin_right90();
          }
          else if (carDirection == "W") {
            //spin180();
          }
          else if (carDirection == "E") {

          }

        }
        else if (yCurrent < yTarget) {
          if (carDirection == "S") {
            spin_left90();
          }
          else if (carDirection == "W") {
            spin_right90();
          }
          else if (carDirection == "N") {

          } else if (carDirection == "E") {

          }
        }
      }

      if (getUltrasonicDistance() < 18) { // obstacles
        // Four Boundary Cases
        if (carDirection == "N" && xCurrent == 5 || carDirection == "W" && yCurrent == 5 || carDirection == "S" && xCurrent == 0 || carDirection == "E" && yCurrent == 0) {
          spin_left90();
        } else {
          spin_right90();
        }

      }


      Serial1.print("PUB|LF:");
      Serial1.print(xCurrent);
      Serial1.print(",");
      Serial1.print(yCurrent);
      Serial1.print(",");
      Serial1.println(carDirection);

      String coordianteMessage = "Mode: \nLine Following\n";
      coordianteMessage += xCurrent;
      coordianteMessage += ",";
      coordianteMessage += yCurrent;
      coordianteMessage += ",";
      coordianteMessage += carDirection;

      printToOLED(coordianteMessage, 2);

    } else {
      // follow the line otherwise
      linefollow(pos);
    }
  }
}

void lineFollowingChageCoordinate(String payload)
{

  Serial.print("Change Coordinate: ");
  Serial.println(payload);

  char LF_delim[] = ",";

  int xNewTarget = 0;
  int yNewTarget = 0;
  String newDirection = "";

  // create char* that keeps str for subsequent operations
  char* c_str = new char[payload.length() + 1];
  strcpy(c_str, payload.c_str());

  // The C library function char *strtok(char *str, const char *delim)
  // breaks string str into a series of tokens using the delimiter delim.
  char* sub_str = strtok(c_str, LF_delim);

  xNewTarget = atoi((char *)sub_str);

  sub_str = strtok(NULL, LF_delim);

  yNewTarget = atoi((char *)sub_str);

  sub_str = strtok(NULL, LF_delim);

  if (sub_str != NULL) {
    newDirection = String(sub_str);
  }

  Serial.print(xNewTarget);
  Serial.print(" ");
  Serial.print(yNewTarget);
  Serial.print(" ");

  char direction_c = newDirection[0];
  if (direction_c != '\0' && (direction_c == 'N' || direction_c == 'E' || direction_c == 'S' || direction_c == 'W'))
  {
    Serial.println(direction_c);
    lineFollowingSetCoordinate(xNewTarget, yNewTarget, String(direction_c));
  }
  else
  {
    Serial.println("Same Direction!");
    lineFollowingSetCoordinate(xNewTarget, yNewTarget, "");
  }

}


void lineFollowingSetCurrentCoordinate(String payload)
{

  Serial.print("Set Coordinate: ");
  Serial.println(payload);

  char LF_delim[] = ",";

  int xNewCurrent = 0;
  int yNewCurrent = 0;
  String newDirection = "";

  // create char* that keeps str for subsequent operations
  char* c_str = new char[payload.length() + 1];
  strcpy(c_str, payload.c_str());

  // The C library function char *strtok(char *str, const char *delim)
  // breaks string str into a series of tokens using the delimiter delim.
  char* sub_str = strtok(c_str, LF_delim);

  xNewCurrent = atoi((char *)sub_str);

  sub_str = strtok(NULL, LF_delim);

  yNewCurrent = atoi((char *)sub_str);

  sub_str = strtok(NULL, LF_delim);

  if (sub_str != NULL) {
    newDirection = String(sub_str);
  }

  Serial.print(xNewCurrent);
  Serial.print(" ");
  Serial.print(yNewCurrent);
  Serial.print(" ");

  char direction_c = newDirection[0];
  if (direction_c != '\0' && (direction_c == 'N' || direction_c == 'E' || direction_c == 'S' || direction_c == 'W'))
  {
    Serial.println(direction_c);
    xCurrent = xNewCurrent;
    yCurrent = yNewCurrent;
    carDirection = direction_c;
  }
  else
  {
    Serial.println("Same Direction!");
    xCurrent = xNewCurrent;
    yCurrent = yNewCurrent;
  }

  String coordianteMessage = "Mode: \nLine Following\n";
  coordianteMessage += xCurrent;
  coordianteMessage += ",";
  coordianteMessage += yCurrent;
  coordianteMessage += ",";
  coordianteMessage += carDirection;

  printToOLED(coordianteMessage, 2);
}

void lineFollowingSetCoordinate(int x, int y, String direction)
{
  xTarget = x;
  yTarget = y;

  if (direction != "")
  {
    carDirection = direction;
  }

  lastError = 0;
  total_count = 0;
  count = 0;
  pos = 0; // car position
  Serial.println("Coordinate Chage Success!");
  Serial.print(xTarget);
  Serial.print(",");
  Serial.print(yTarget);
  if (direction != "")
  {
    Serial.print(",");
    Serial.println(direction);
  }


  Serial1.print("PUB|LF:");
  Serial1.print(xTarget);
  Serial1.print(",");
  Serial1.print(yTarget);
  if (direction != "")
  {
    Serial1.print(",");
    Serial1.println(direction);
  }
  else
  {
    Serial1.println();
  }
}

//Light Tracking Method
///////////////////////////////////////////////////////////////////////////
void getLight() {
  int left = analogRead(A3);
  int right = analogRead(A4);
  int diff = left - right + 120;
  light = (diff ) / 200.0 * (-1) ;
}

void lightTracking() {
  r_qti = get_qti(A0);
  m_qti = get_qti(A1);
  l_qti = get_qti(A2);

  getLight();
  linefollow(light);
}

//Finding Red Spot Method
///////////////////////////////////////////////////////////////////////////
void findingRedSpot() {
  xCurrent = 0;
  yCurrent = 0;
  xTarget = 5;
  yTarget = 0;

  while(!redSpotDetected && !(xCurrent == 5 && yCurrent == 2)){
    readData();
    lineFollowing();
    if(arrivedTarget){    // Change target coordinates, so the car follows an 'S' shape route
      yTarget += 1;
      if(xTarget == 5){
        xTarget = 0;
      }else if(xTarget == 0){
        xTarget = 5;
      }
      arrivedTarget = false;
    }
  }

  stay();
  if(redSpotDetected){
    String msg = "Red spot found in: ";
    msg += xCurrent ;
    msg += ", ";
    msg += yCurrent;
    printToOLED(msg, 1);
  }else{
    printToOLED("Red spot is not found", 1);
  }

}

// Reset ColorPAL; see ColorPAL documentation for sequence
void reset() {
  delay(200);
  pinMode(sio, OUTPUT);
  digitalWrite(sio, LOW);
  pinMode(sio, INPUT);
  while (digitalRead(sio) != HIGH);
  pinMode(sio, OUTPUT);
  digitalWrite(sio, LOW);
  delay(80);
  pinMode(sio, INPUT);
  delay(waitDelay);
}

// Read color pixels data
void readData() {
  char buffer[32];

  if (serin.available() > 0) {
    // Wait for a $ character, then read three 3 digit hex numbers
    buffer[0] = serin.read();
    if (buffer[0] == '$') {
      for(int i = 0; i < 9; i++) {
        while (serin.available() == 0);     // Wait for next input character
        buffer[i] = serin.read();
        if (buffer[i] == '$')               // Return early if $ character encountered
          return;
      }
      parseAndPrint(buffer);
      delay(10);
    }
  }
}

// Parse the hex data into integers
void parseAndPrint(char * data) {
  sscanf (data, "%3x%3x%3x", &red, &grn, &blu);
  char buffer[32];
  sprintf(buffer, "R%4.4d G%4.4d B%4.4d", red, grn, blu);
  if (red > 140 && grn < 80 && blu < 130) {   // Pixel data for Red color
    redSpotDetected = true;
    Serial.println("RED is detected");
  }
}



//MQTT Related Method
///////////////////////////////////////////////////////////////////////////
MqttMsg parse_mqtt_msg(String str) {

  MqttMsg mqtt_msg;
  char delim[] = "|";

  // create char* that keeps str for subsequent operations
  char* c_str = new char[str.length() + 1];
  strcpy(c_str, str.c_str());

  // The C library function char *strtok(char *str, const char *delim)
  // breaks string str into a series of tokens using the delimiter delim.
  char* sub_str = strtok(c_str, delim);

  //Check Topic
  if (strcmp(sub_str, "Topic") != 0) {
    return mqtt_msg; // str does not contain MQTT message
  }

  sub_str = strtok(NULL, delim);

  if (sub_str == NULL) {
    return mqtt_msg; // str not complete
  }
  mqtt_msg.topic = String(sub_str);

  //Skip Tab
  sub_str = strtok(NULL, delim);

  //Check Payload
  sub_str = strtok(NULL, delim);
  if (sub_str == NULL) {
    return mqtt_msg; // str not complete
  }
  sub_str = strtok(NULL, delim);

  mqtt_msg.payload = String(sub_str);

  return mqtt_msg;
}

void sendToMQTT(String topic, String payload)
{ //example: sendToMQTT("Topic","Payload");
  char topic_c[1024];
  char payload_c[1024];
  topic.toCharArray(topic_c, topic.length());
  payload.toCharArray(payload_c, payload.length());
  Serial1.print("PUB|");
  Serial1.print(topic);
  Serial1.print(":");
  Serial1.println(payload);
}

//OLED Method
///////////////////////////////////////////////////////////////////////////
void initOLED() {
  display.fillScreen(BLACK);
  display.setCursor(15, 5);
  display.setTextColor(WHITE);
  display.setTextSize(2);
  display.println("HKUEEE");
  display.setCursor(0, 25);
  display.setTextColor(CYAN);
  display.setTextSize(2);
  display.println("IDP 2018");
  display.setCursor(15, 50);
  display.setTextColor(YELLOW);
  display.setTextSize(2);
  display.println("Final");
}

void printToOLED(String msg, int fontSize)
{
  display.fillScreen(BLACK);
  display.setTextColor(WHITE);
  display.setTextSize(fontSize);
  display.setCursor(0, 0);
  display.print(msg);
}

//Main Loop
///////////////////////////////////////////////////////////////////////////
void loop() {
  // Serial.println() outputs to Serial Monitor
  // Serial1.println() outputs to ESP8266

  /* Get MQTT messages from ESP */
  while (Serial1.available()) {
    String serial_str;
    serial_str = Serial1.readStringUntil('\n');
    Serial.println(serial_str);

    MqttMsg mqtt_msg = parse_mqtt_msg(serial_str);

    if (mqtt_msg.topic[0] != '\0' && mqtt_msg.payload[0] != '\0')
    {
      Serial.print(F("Arduino Received - Topic: "));
      Serial.print(mqtt_msg.topic);
      Serial.print(F("\tPayload: "));
      Serial.println(mqtt_msg.payload);
      Serial1.println("PUB|Debug:ACK"); //Pub ACK to MQTT after successfully received a message from ESP

      char topic_c[100];
      char payload_c[100];

      mqtt_msg.payload.toCharArray(payload_c, mqtt_msg.payload.length());

      //Car 1
      if (mqtt_msg.topic == "JARVIS/server/1/MODE" || mqtt_msg.topic == "JARVIS/server/MODE")
      //Car 2
      //if (mqtt_msg.topic == "JARVIS/server/2/MODE" || mqtt_msg.topic == "JARVIS/server/MODE")
      {
        //Determine Mode
        //Serial.println(mqtt_msg.payload);
        if (strcmp(payload_c, "D") == 0)
        {
          mode = "D";
          Serial.println("Mode Changed: DEFAULT");
          sendToMQTT("Mode", "Default");
          printToOLED("Mode: \nDefault", 2);
        }
        else if (strcmp(payload_c, "LF") == 0)
        {
          mode = "LF";
          Serial.println("Mode Changed: Line Following");
          Serial1.print("PUB|LF:");
          Serial1.print(xCurrent);
          Serial1.print(",");
          Serial1.print(yCurrent);
          Serial1.print(",");
          Serial1.println(carDirection);
          sendToMQTT("Mode", "Line Following");
        }
        else if (strcmp(payload_c, "RED") == 0)
        {
          mode = "RED";
          Serial.println("Mode Changed: Finding Red Spot");
          sendToMQTT("Mode", "Finding Red Spot");
          printToOLED("Mode: \nFinding Red Spot", 2);
        }
        else if (strcmp(payload_c, "LT") == 0)
        {
          mode = "LT";
          Serial.println("Mode Changed: Light Tracking");
          sendToMQTT("Mode", "Light Tracking");
          printToOLED("Mode: \nLight Tracking", 2);
        }
        else if (strcmp(payload_c, "TR") == 0)
        {
          spin_right90();
          Serial.println("Turn Right");
          sendToMQTT("Debug", "Turn Right");
          printToOLED("Turn Right", 2);
        }
        else if (strcmp(payload_c, "TL") == 0)
        {
          spin_left90();
          Serial.println("Turn Left");
          sendToMQTT("Debug", "Turn Left");
          printToOLED("Turn Left", 2);
        }
        else
        {
          Serial.println("No such mode :(");
          sendToMQTT("Mode", "Mode NA");
        }
      }

      //Collision
      //Car 1
      else if (mqtt_msg.topic == "JARVIS/server/1/Collision" || mqtt_msg.topic == "JARVIS/server/Collision")
      //Car 2
      //else if (mqtt_msg.topic == "JARVIS/server/2/Collision" || mqtt_msg.topic == "JARVIS/server/Collision")
      {
        if (strcmp(payload_c, "Stop") == 0)
        {
          mode = "D";
          sendToMQTT("Collision", "Stop");
          printToOLED("Collision: \nStop", 2);
        }
        else if (strcmp(payload_c, "Turn") == 0)
        {
          // Four Boundary Cases
          if (carDirection == "N" && xCurrent == 5 || carDirection == "W" && yCurrent == 5 || carDirection == "S" && xCurrent == 0 || carDirection == "E" && yCurrent == 0) {
            spin_left90();
          } else {
            spin_right90();
          }
          sendToMQTT("Collision", "Turn");
          printToOLED("Collision: \nTurn", 2);
        }
        else if (strcmp(payload_c, "Move") == 0)
        {
          mode = "LF";
          sendToMQTT("Collision", "Move");
          printToOLED("Collision: \nMove", 2);
        }
        else if (strcmp(payload_c, "Next") == 0)
        {
          sendToMQTT("Collision", "Next");
          printToOLED("Collision: \nNext", 2);

          count = 0;

          while (count < 10)
          {
            r_qti = get_qti(A0);
            m_qti = get_qti(A1);
            l_qti = get_qti(A2);

            getpos(l_qti, m_qti, r_qti, pos);
            // count how many interceptions have been passed
            if (l_qti > thre3 && m_qti > thre3 && r_qti > thre3) {

              count++;
            }
            else
            {
              linefollow(pos);
            }
          }
          forward();
          delay(250);
          if (carDirection == "N") {
            yCurrent++;
          } else if (carDirection == "S") {
            yCurrent--;
          } else if (carDirection == "E") {
            xCurrent++;
          } else if (carDirection == "W") {
            xCurrent--;
          }
        }

        Serial1.print("PUB|LF:");
        Serial1.print(xCurrent);
        Serial1.print(",");
        Serial1.print(yCurrent);
        Serial1.print(",");
        Serial1.println(carDirection);
      }

      //Line Following: Set Current Coordinate
      //Car 1
      else if (mqtt_msg.topic == "JARVIS/server/1/LF_C")
      //Car 2
      //else if (mqtt_msg.topic == "JARVIS/server/2/LF_C")
      {
        lineFollowingSetCurrentCoordinate(mqtt_msg.payload);
      }

      //Line Following: Change Coordinate
      //Car 1
      else if (mqtt_msg.topic == "JARVIS/server/1/LF" || mqtt_msg.topic == "JARVIS/server/LF")
        //Car 2
      //else if (mqtt_msg.topic == "JARVIS/server/2/LF" || mqtt_msg.topic == "JARVIS/server/LF")
      {
        lineFollowingChageCoordinate(mqtt_msg.payload);
      }
    }
  }

  if (mode == "D")
  {
    stay();
  }
  else if (mode == "LF")
  {
    lineFollowing();
    /*
      Serial1.print("PUB|LF:");
      Serial1.print(xCurrent);
      Serial1.print(",");
      Serial1.print(yCurrent);
      Serial1.print(",");
      Serial1.println(carDirection);
    */
  }
  else if (mode == "LT")
  {
    lightTracking();
  }
  else if (mode == "RED")
  {
    findingRedSpot();
  }
}