/*
 *  Transmitter
 *
 *
 *
 *
 */

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

const int pLED = 12;
const int rLED = 7;
const int gLED = 8;

const int pSwitch_1 = 2;
const int pSwitch_2 = 3;
const int pSwitch_3 = 4;
const int pSwitch_4 = 5;
const int pSwitch_5 = 6;

int i;
int j;
String incomingString;
String incomingStringrelay;
boolean state;
boolean staterelay;
int state5 = 1;
int prevstate = 1;
String relay1 = "OFF";
String relay2 = "OFF";
String relay3 = "OFF";
String relay4 = "ON";
boolean relayno1 = false;
boolean relayno2 = false;
boolean relayno3 = false;
boolean relayno4 = false;

void setup()
{
  // put your setup code here, to run once:

  pinMode(pLED, OUTPUT);
  pinMode(rLED, OUTPUT);
  pinMode(gLED, OUTPUT);
  pinMode(13, OUTPUT);
  digitalWrite(13, LOW);

  pinMode(pSwitch_1, INPUT_PULLUP);
  pinMode(pSwitch_2, INPUT_PULLUP);
  pinMode(pSwitch_3, INPUT_PULLUP);
  pinMode(pSwitch_4, INPUT_PULLUP);
  pinMode(pSwitch_5, INPUT_PULLUP);
  Serial.begin(115200);
  // delay(5000);
  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
}

void getFeedback(String excpt_str)
{
  i = 0;
  incomingString = "";
  state = true;

  digitalWrite(gLED, LOW);
  digitalWrite(rLED, LOW);

  // Serial.print("Waiting for feedback");
  while (1)
  {
    if (Serial.available() > 0)
    {
      incomingString = Serial.readString();
      if (incomingString.indexOf(excpt_str) > 0)
      {
        state = true;
        break;
      }
    }
    if (i > 60)
    {
      state = false;
      break;
    }
    delay(100);
    digitalWrite(pLED, !digitalRead(pLED));
    i++;
  }

  if (state)
  {
    digitalWrite(gLED, HIGH);
    digitalWrite(rLED, LOW);
    digitalWrite(pLED, LOW);

    // Serial.println("Received");
  }
  else
  {
    digitalWrite(gLED, LOW);
    digitalWrite(rLED, HIGH);
    digitalWrite(pLED, LOW);
    // Serial.println("Not Received");
  }
}

void getFeedbackrelay(String excpt_strr, boolean relayno)
{
  j = 0;
  incomingStringrelay = "";
  staterelay = true;

  digitalWrite(gLED, LOW);
  digitalWrite(rLED, LOW);

  // Serial.print("Waiting for feedback");
  while (1)
  {
    if (Serial.available() > 0)
    {
      incomingStringrelay = Serial.readString();
      if (incomingStringrelay.indexOf(excpt_strr) > 0)
      {
        staterelay = true;
        break;
      }
    }
    if (j > 60)
    {
      staterelay = false;
      break;
    }
    delay(100);
    digitalWrite(pLED, !digitalRead(pLED));
    j++;
  }

  if (staterelay)
  {
    digitalWrite(gLED, HIGH);
    digitalWrite(rLED, LOW);
    digitalWrite(pLED, LOW);
    relayno = !relayno;

    // Serial.println("Received");
  }
  else
  {
    digitalWrite(gLED, LOW);
    digitalWrite(rLED, HIGH);
    digitalWrite(pLED, LOW);
    // Serial.println("Not Received");
  }
}


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

  if (digitalRead(pSwitch_5) == LOW)
  {
    state5 = (digitalRead(pSwitch_5));
    if (state5 != prevstate)
    {
      Serial.println("AT+SEND=2,6,R5");
      tone(10, 2700, 5000);
      getFeedback("FR5");
      prevstate = state5;
    }
    if (state)
    {
      int bat = 30;
      display.clearDisplay();
      display.setTextColor(WHITE);
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("Battery  ");
      display.print(bat);
      display.print("%");
      display.setCursor(0, 10);
      display.print("                ");
      display.setCursor(0, 20);
      display.print("E-STOP");
      relay1 = "OFF";
      relay2 = "OFF";
      relay3 = "OFF";
      relay4 = "OFF";
    }

    if (!state)
    {
      int bat = 30;
      display.clearDisplay();
      display.setTextColor(WHITE);
      display.setTextSize(1);
      display.setCursor(0, 0);
      display.print("Battery  ");
      display.print(bat);
      display.print("%");
      display.setCursor(0, 10);
      display.print("Transmission Failed");
    }
  }
  else
  {
    if (digitalRead(pSwitch_1) == LOW)
    {
      Serial.println("AT+SEND=2,6,R1");
      getFeedbackrelay("FR1",relayno1);
      if (relayno1)
      {
        relay1 = "ON";
      }
      else
      {
        relay1 = "OFF";
      }
    }
    else if (digitalRead(pSwitch_2) == LOW)
    {
      Serial.println("AT+SEND=2,6,R2");
      getFeedbackrelay("FR2",relayno2);
      if (relayno2)
      {
        relay2 = "ON";
      }
      else
      {
        relay2 = "OFF";
      }
    }
    else if (digitalRead(pSwitch_3) == LOW)
    {
      Serial.println("AT+SEND=2,6,R3");
      getFeedbackrelay("FR3",relayno3);
      if (relayno3)
      {
        relay3 = "ON";
      }
      else
      {
        relay3 = "OFF";
      }
    }
    else if (digitalRead(pSwitch_4) == LOW)
    {
      Serial.println("AT+SEND=2,6,R4");
      getFeedbackrelay("FR4",relayno4);
      if (relayno4)
      {
        relay4 = "ON";
      }
      else
      {
        relay4 = "OFF";
      }
    }

    int bat = 30;
    display.clearDisplay();
    display.setTextColor(WHITE);
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("Battery  ");
    display.print(bat);
    display.print("%");
    display.setCursor(0, 10);
    display.print("RELAY: 1 ");
    display.print(relay1);
    display.setCursor(0, 20);
    display.print("RELAY: 2 ");
    display.print(relay2);
    display.setCursor(0, 30);
    display.print("RELAY: 3 ");
    display.print(relay3);
    display.setCursor(0, 40);
    display.print("RELAY: 4 ");
    display.print(relay4);
    prevstate = 1;
  }

  display.display();
}