#include <SoftwareSerial.h>
#include <string.h>

int bluetoothTx = 3;  // TX-O pin of bluetooth mate, Arduino D2
int bluetoothRx = 2;  // RX-I pin of bluetooth mate, Arduino D3
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
int connd = 0;
int enable = 1;

void setup()
{
  Serial.begin(9600);  // Begin the serial monitor at 9600bps
  char inChar;
  bluetooth.begin(115200);  // The Bluetooth Mate defaults to 115200bps
  delay(100);  // Short delay, wait for the Mate to send back CMD
  bluetooth.println("U,9600,N");  // Temporarily Change the baudrate to 9600, no parity
  bluetooth.begin(9600);  // Start bluetooth serial at 9600
/*  bluetooth.print("AT+ROLE0");
  delay(50);
  bluetooth.print("AT+MODE1");
  delay(50);
  bluetooth.print("AT+TYPE0");
  delay(50);
  bluetooth.print("AT+NOTI1");
  delay(50);
  bluetooth.print("AT+PASS123456");
  delay(50);
  bluetooth.print("AT+FILT0");
  delay(50);
  bluetooth.print("AT+ERASE");
  delay(50);
  bluetooth.print("AT+CLEAR");
  delay(50);
  bluetooth.print("AT+POWE3");
  delay(50);
  bluetooth.print("AT+NAMELOCK");
  delay(50);
  bluetooth.print("AT+RESET"); */
  delay(300);
  bluetooth.print("AT+START");
  delay(100);
  int i = 0;
  String inString = String("");

  while (bluetooth.available()) {
      inString += (char)bluetooth.read();
  }
  //Serial.print(inString);
  Serial.print(" Setup Complete. ");
}

void pollDist(){
    delay(3000);
    String distance;
    int startRead;
    char inChar;
    String inString = String("");
    while(connd){
      delay(2000);
      if (readyTs){
        bluetooth.print("AT+RSSI?");
      }
        distance = String("");
        inString = String("");
        while (bluetooth.available()){
            inChar = (char)bluetooth.read();
            inString += inChar;
            if (inChar == '-'){
                startRead = 1;
                distance = String("");
            }
            else if(startRead == 1 && inChar != 'O') {
                distance += inChar;
            }
            else if (startRead ==1 && inChar == 'O') {
                startRead = 0;
            }
        }

        int intDistance = 0;
        intDistance = String(distance).toInt();
        if (inString.indexOf("OK+LOST") != -1) {
            connd = 0;
            Serial.println(" connection lost ");
            return;
        }


        if (inString.indexOf("connectionStatus.") != -1) {
            bluetooth.print("good.");
        }


        if (inString.indexOf("!disable.") != -1) {
            enable = 0;
            Serial.println("disable pressed");
        }
        if (inString.indexOf("!enable.") != -1) {
            enable = 1;
            Serial.println("enable pressed");
        }

        if (intDistance < 45 && enable && intDistance > 10) {
            Serial.println(" distance " + distance + "Access Granted. ");
        }
        if (intDistance > 65) {
            Serial.println(" distance " + distance + "DOOR LOCKED BRO.");
        }
        else if (!enable) {
          Serial.println("yo junk has been disabled");
        }
    }
}



void loop()
{
  char inChar;
  String inString;
    while (bluetooth.available()) {
        delay(50);
        inChar = (char)bluetooth.read();

        if (inChar == '+' || inChar == '!'){
            inString = String("");
            }
        else {
            inString += inChar;

        }
        if (inString == "CONN") {
            Serial.print(" Connected Now ");
            connd = 1;
            pollDist();

        }
        if (inString == "LOST") {
            connd = 0;
        }
        if (inString == "connectionStatus.") {
            bluetooth.print("good.");
        }
    }
}