#include <WiFi.h>
#include <PubSubClient.h>

#define RXD2 16    //ESP32 UART2 pins
#define TXD2 17



// Add Wi-Fi and server info
const char* ssid ="NETGEAR99_2GEXT";   // Add your Wi-Fi ssid
const char* pass= "3237370742";   // Add Wi-Fi PW
const char* brokerUser= ("ENGR");   // Add the user name you selected from the server page
const char* brokerPass= ("engr");   // Add the PW you selected from the server page
const char* broker= ("broker.emqx.io");
const char* outTopic = ("test1");    //Add the name you selected for your subscription
const char* inTopic= ("test1");      // Add the name you selected for your subscription
char messages[100];
String joy_d; //String to store data from server
String MegaData;
int Down=13;
int Up=12;
int Left=14;
int Right=27;

WiFiClient espClient;
PubSubClient client(espClient);


void setupwifi() {
  delay(100);
  Serial.println("\nConnecting to");
  Serial.println(ssid);

  WiFi.begin(ssid, pass);

  while (WiFi.status() != WL_CONNECTED) {
    delay(100);
    Serial.print("-");
  }
  Serial.print("\nConnected to");
  Serial.println(ssid);
}

void reconnect() {
  while (!client.connected()) {
    Serial.print("\nConnecting to");
    Serial.println(broker);
    if (client.connect("Insert a client ID", brokerUser, brokerPass)) {
      Serial.println("\nConnected to");
      Serial.println(broker);
      client.subscribe(inTopic);
    } else {
      Serial.println("\nTrying connect again");
      delay(5000);
    }
  }
}


void callback(char* topic, byte* payload, unsigned int length) {  // This portion reads data from the server
  String messageout = "";
  for (int i = 0; i < length; i++) {
    ((char) payload[i]);
    messageout += (char) payload[i];
    if (messageout.indexOf("LCD_R") >= 0) {    //If anywhere in the server data there are consecutive characters xval=255, string joy_d="Right"
      Serial1.println("R");
      
    }
   if (messageout.indexOf("LCD_L") >= 0) {    //If anywhere in the server data there are consecutive characters xval=0, string joy_d="Left"
       Serial1.println("L");
    }
    if (messageout.indexOf("LCD_U") >= 0) {
       Serial1.println("U");
    }
   if (messageout.indexOf("LCD_D") >= 0) {
       Serial1.println("D");
    }
     if (messageout.indexOf("S1") >= 0) {
       Serial1.println("sensor1");
    }
      if (messageout.indexOf("S2") >= 0) {
       Serial1.println("sensor2");
    }
   if (messageout.indexOf("S3") >= 0) {
       Serial1.println("sensor3");
    }
    if (messageout.indexOf("S4") >= 0) {
       Serial1.println("sensor4");
    }
    if (messageout.indexOf("S5") >= 0) {
       Serial1.println("sensor5");
    }
     else if (messageout.indexOf("S6") >= 0) {
       Serial1.println("sensor6");
    }
   
   
  }
}

void setup() {
  
pinMode(Left,INPUT);
pinMode(Right,INPUT);
pinMode(Up,INPUT);
pinMode(Down,INPUT);
 Serial.begin(115200);
Serial2.begin(9600, SERIAL_8N1, RXD2, TXD2);  // ESP32 UART 2 pins
Serial1.begin(9600, SERIAL_8N1, RXD2, TXD2); //Mega UART 1 pins
  setupwifi();
  client.setServer(broker, 1883);
  client.setCallback(callback);


}


 void Joystick() {
  
  // put your main code here, to run repeatedly:
if (digitalRead(Left)==LOW){
  Serial.println("Left");
  String msg = "Left";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
  delay(250);
    msg = "Stop";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
delay(250);
}
else if (digitalRead(Right)==LOW){
  Serial.println("Right");
  String msg = "Right";
  msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
   delay(250);
    msg = "Stop";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
delay(250);
}
else if (digitalRead(Up)==LOW){
  Serial.println("Up");
  String msg = "Up";
   msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
   delay(250);
   msg = "Stop";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
delay(250);

}
else if (digitalRead(Down)==LOW){
  Serial.println("Down");
String msg = "Down";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
   delay(250);
   msg = "Stop";
    msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
delay(250);
}
//else {
//   Serial.println("Stop");
// String msg = "Stop";
//    msg.toCharArray(messages, 100);
//      client.publish(outTopic, messages);
//delay(500);
//}
}  
void loop() {
  if (!client.connected()) {
    reconnect();
  }
  client.loop();
Joystick();
 client.loop();
  if (Serial2.available()) {
   String  MegaData = Serial2.readString();
    Serial.print(MegaData);
    if (MegaData.indexOf("Done") >= 0) {
      String msg = "Ship_destroy";
       msg.toCharArray(messages, 100);
      client.publish(outTopic, messages);
      delay(500);
    }
  }
}