#include <Arduino.h>
#include <SoftwareSerial.h>
#include <ESP8266_Simple.h>

// Setup SoftwareSerial to pins (RX, TX)
ESP8266_Simple wifi(8,9);

// Motor Pins
const int left = 11;
const int middle = 3;
const int right = 5;

// Values
const int idle = 2500; //Time in between vibrations
const int active = 1500; //Time in ms the motor vibrates each cicle
const int strength = 800; //Regulates voltage (WARNING: Max 4V!)
String proxRouter ; //the number of the router thats closest.,,

void setup()
{
  Serial.begin(115200);
  Serial.println("ESP8266 Demo Sketch");
  wifi.begin(9600);
  pinMode(left,OUTPUT);
  pinMode(middle,OUTPUT);
  pinMode(right,OUTPUT);
  // A blank line just for debug formatting
  Serial.println();
}

void loop()
{
  proxRouter = "No zone in range";
  String scan_results;
  String Zone1 = "Zone 1";
  String Zone2 = "Zone 2";
  String Zone3 = "Zone 3";
  byte responseCode;
  char buffer[500];
  // Debug formatting
  Serial.println();
  Serial.print("Getting Access Points: ");
  // Send to scan networks command to the ESP8266
  if((responseCode = wifi.sendCommand(F("AT+CWLAP" ), buffer, sizeof(buffer))) == ESP8266_OK)
  {
    // The command worked:
    Serial.println("OK");
    Serial.println(buffer);
    scan_results = buffer;
    int strongest_RSSI;
    String strongest_SSID;
    // Loop through each line, the start of a line is defined by a + .
    for (int line = scan_results.indexOf('+'); line < scan_results.length();)
    {
      // At the start of the loop, proxRouter is wiped to prevent motors being
      // activated when one of our routers has been the proxRouter in the previous loop(s).
      // To retreive the signal strength and SSID the index of multiple ','
      // are found and used to only get the usefull parts of the output.
      int commaIndex = scan_results.indexOf(',', line);
      int secondCommaIndex = scan_results.indexOf(',', commaIndex + 1);
      int thirdCommaIndex = scan_results.indexOf(',', secondCommaIndex + 1);
      int fourthCommaIndex = scan_results.indexOf(',', thirdCommaIndex + 1);
      int fifthCommaIndex = scan_results.indexOf(',', fourthCommaIndex + 1);
      String SSID_line_raw = scan_results.substring(commaIndex + 1, secondCommaIndex);
      String RSSI_line_noInt = scan_results.substring(secondCommaIndex +1, thirdCommaIndex);
      String SSID_line_raw2 = SSID_line_raw;
      SSID_line_raw2.remove(SSID_line_raw.length() -1);
      String SSID_line = SSID_line_raw2;
      SSID_line.remove(0, 1);
      int RSSI_line = abs(RSSI_line_noInt.toInt());
      // At the start of the first line, the strongest SSID cannot be encoutered yet so:
      if(line == 0){
        strongest_RSSI = 100;
        strongest_SSID = "No Zones in range";
      }
      // Find the stongest network in the scan.
      if ((RSSI_line < strongest_RSSI) && (SSID_line.equals(Zone1) || SSID_line.equals(Zone2) || SSID_line.equals(Zone3))){
        strongest_RSSI = RSSI_line;
        strongest_SSID = SSID_line;
        proxRouter = SSID_line;
      }
      // Iterate to the next line.
      line = scan_results.indexOf('+', line + 1);
    }
    // Print results. Later this can be saved to output for vib. motors.
    Serial.print(strongest_SSID);
    Serial.print(", strength: ");
    Serial.println(strongest_RSSI);
    if(proxRouter.equals(Zone1))
    {
      Serial.println("Starting vibration Zone 1");
      digitalWrite(left, HIGH);
      delay(active);
      digitalWrite(left, LOW);
      Serial.println("Stopping vibration Zone 1");
    }
    else if(proxRouter.equals(Zone2))
    {
      Serial.println("Starting vibration Zone 2");
      digitalWrite(middle, HIGH);
      delay(active);
      digitalWrite(middle, LOW);
      Serial.println("Stopping vibration Zone 2");
    }
   else if(proxRouter.equals(Zone3))
   {
      Serial.println("Starting vibration Zone 3");
      digitalWrite(right, HIGH);
      delay(active);
      digitalWrite(right,LOW);
      Serial.println("Stopping vibration Zone 3");
    }
  }
  // Debug formatting
  Serial.println();
  // Wait before scanning again.
  delay(5);
}