 void motorRightEncoderInterruptHandler();
 void updateClient();
 
-
 /////////////////////
 // Pin Definitions //
 /////////////////////

boolean driverAssist = false;
bool HeartBeatRcvd = false;
String closeConnectionHeader = "";
#define MAX_SRV_CLIENTS 10               // maximum client connections
WiFiClient serverClients[MAX_SRV_CLIENTS];
int currentClient = 0;
boolean pingOn = false;
long nextBoredBotEvent = 0;
int boredBotTimeout = 60000;             //in ms
boolean leftBumperHit = false;
boolean rightBumperHit = false;
boolean autoMode = false; 				 // drive mode with no client connected
long autoModeNextUpdate = 0;
long autoModeNextEvent = 0;
//#define Diag                           // if not defined disables serial communication after initial feedback
long timerPing;


void Stop(void)
{
  motors.manualDrive(0,0);
  setColor(RgbColor(0,0,0));             // turn off LED's to save power
  pingOn = false;                        // turn off ping to save power
  driverAssist = false;
}

void CheckHeartBeat(void)
{
  if (HeartBeatRcvd == true)
  {
    HeartBeatRcvd = false;
    nextBoredBotEvent = millis() + boredBotTimeout; // reset bored bot timer
  }
  else
  {
    Stop();
  }
}

void setup()
{
  system_update_cpu_freq(160);           // set cpu to 80MHZ or 160MHZ !
  system_phy_set_max_tpw(10); // 0 - 82 radio TX power
  initHardware();
  setupWiFi();
  setupWebsocket();
  HeartBeatTicker.attach_ms(1000, CheckHeartBeat);
  closeConnectionHeader += F("HTTP/1.1 204 No Content\r\nConnection: Close\r\n\r\n");
  nextBoredBotEvent = millis() + boredBotTimeout;
}

void loop()
{
  // time dependant functions here
  checkBoredBot();
  checkAutoMode();
  if (pingOn){
   getDistance();                       // ping pulse/update function must be called to ping
   distance = getMedian();
   if (driverAssist){
        if (distance < 200){            // too close bounce back
          setColor(RgbColor(255,0,0));
          motors.manualDrive(0,500);
          }
    }
  }

  if (driverAssist){
  testBumper();
  }
   dnsServer.processNextRequest();      // update DNS requests

   // client functions here
  while (server.hasClient()){
    for(uint8_t i = 0; i < MAX_SRV_CLIENTS; i++){
      //find free/disconnected spot
      if (!serverClients[i] || !serverClients[i].connected()){
        if(serverClients[i]) serverClients[i].stop();
        serverClients[i] = server.available();
        return;
      }
    }
    //no free/disconnected spot so reject
   // WiFiClient serverClient = server.available();
   // serverClient.stop();
  }
  //check clients for data
  String req = "";
  for(uint8_t i = currentClient; i < MAX_SRV_CLIENTS; i++){// start at current client to keep in order
    if (serverClients[i] && serverClients[i].connected()){
        if (serverClients[i].available()){
          req = serverClients[i].readStringUntil('\r');   // Read the first line of the request
          serverClients[i].flush();
          currentClient = i;
          break;
        }
    }
    currentClient = 0;
  }
  executeRequest(req);
  //Loop WebSocket Server
  webSocket.loop();
}
/**
 * A faster version of exeReq for websockets.
 * Features WS response and a shorter command list.
 */
void fastExecuteRequest(String req){
  //Empty request tripwire
  if(!req.length()){
    //empty request
    return;
  }
  HeartBeatRcvd = true;
  if (req.indexOf("/HB") != -1){
        //Fast Catch and Release, HB handled above.        
        updateClient();
        yield();
        return;
      }
  //Serial.println("\r\n" + req);
  int indexOfX = req.indexOf("/X");
  int indexOfY = req.indexOf("/Y");
  if (indexOfX != -1 && indexOfY != -1){
    pingOn = true;
    if (req.indexOf("/HBDA") != -1)driverAssist = true;
    if (req.indexOf("/HBDM") != -1)driverAssist = false;
    //serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
    yield();
    String xOffset = req.substring(indexOfX + 2, indexOfX + 8);
    int dX = xOffset.toInt();
    String yOffset = req.substring(indexOfY + 2, indexOfY + 8);
    int dY = yOffset.toInt();
    // driver assist
    if (driverAssist){
      updateBlinkers(dX,dY);
        if (distance < 450 && distance > 199 && dY < 0){
          setColor(RgbColor(90,105,95));
          motors.hardRightTurn();
          dX = 500;
          dY = -100;
        }
        if (distance < 200){
         setColor(RgbColor(255,0,0));
          dY = 500;
        }
        updateClient();
    }else{
      pixelTest();
    }
    motors.manualDrive(dX,dY);
  }
  if (req.indexOf("data,") != -1){
          //serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
          yield();
          req.remove(0,req.indexOf("data,"));
          req.trim();
          motors.startCommandSet(req);          
          return;
      }
}
void executeRequest(String req){
  if (!req.length()){// empty request
      return;
      }
  HeartBeatRcvd = true;                                           // received data, must be connected
  if (autoMode){
		autoMode = false;										 // disable autoMode if client sends packet
		HeartBeatRcvd = false;
		Stop();
  }
  //Serial.println("\r\n" + req);
  int indexOfX = req.indexOf("/X");
  int indexOfY = req.indexOf("/Y");
  if (indexOfX != -1 && indexOfY != -1){
    pingOn = true;
    if (req.indexOf("/HBDA") != -1)driverAssist = true;
    if (req.indexOf("/HBDM") != -1)driverAssist = false;
    serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
    yield();
    String xOffset = req.substring(indexOfX + 2, indexOfX + 8);
    int dX = xOffset.toInt();
    String yOffset = req.substring(indexOfY + 2, indexOfY + 8);
    int dY = yOffset.toInt();
    // driver assist
    if (driverAssist){
      updateBlinkers(dX,dY);
        if (distance < 450 && distance > 199 && dY < 0){
          setColor(RgbColor(90,105,95));
          motors.hardRightTurn();
          dX = 500;
          dY = -100;
        }
        if (distance < 200){
         setColor(RgbColor(255,0,0));
          dY = 500;
        }
    }else{
      pixelTest();
    }
    motors.manualDrive(dX,dY);
  }else{
        String fileString = req.substring(4, (req.length() - 9));
        //Serial.println("\r\n" + fileString);
          if (fileString.indexOf("/HB") != -1){
            pingOn = false;
            driverAssist = false;
            HeartBeatRcvd = true;
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
            yield();
            return;
          }
          if (fileString.indexOf("data,") != -1){
              serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
              yield();
              fileString.remove(0,fileString.indexOf("data,"));
              fileString.trim();
              motors.startCommandSet(fileString);
              return;
          }
          if (fileString.indexOf("feedback") != -1){             // send feedback to drive web page
                int temperature = getCurrentTemperature();
                String s,h;
                s = F("<!DOCTYPE HTML><html><head><meta http-equiv='refresh' content='1'></head><body><script>");
                s += (";var tmp=");
                s += temperature;
                s += (";var dis=");
                s += distance;
                s += (";var kph=");
                s += motors.getSpeed();
                s += (";var movd=");
                s += motors.getTravel();
                s += (";var hed=");
                s += motors.getheading();
                s += (";</script></body></html>");
                h = F("HTTP/1.1 200 OK\r\nContent-Type: text/html\r\nConnection: Close\r\ncontent-length: ");
                h += s.length();
                h += F("\r\n\r\n");
                String ss = h + s;
                serverClients[currentClient].write(ss.c_str(),ss.length());
                yield();
                return;
          }
          if(fileString.indexOf("write,")!=-1){
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
              yield();
              fileString.remove(0,fileString.indexOf("write,"));
              fileString.trim();
              //TODO Save File
              
              return;
          }
          if (fileString.indexOf("/PlayCharge") != -1){
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
            yield();
            motors.playCharge();
            return;
          }
          if (fileString.indexOf("/PlayMarch") != -1){
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
            yield();
            motors.playMarch();
            return;
          }
          if (fileString.indexOf("/PlayMarioTheme") != -1){
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
            yield();
            motors.playMarioMainThem();
            return;
          }
          if (fileString.indexOf("/PlayMarioUnderworld") != -1){
            serverClients[currentClient].write( closeConnectionHeader.c_str(),closeConnectionHeader.length() );
            yield();
            motors.playMarioUnderworld();
            return;
          }
          sendFile(fileString);
        }
}
/**
 * Handles Websocket reception, deciding what needs to be done vs the type of message.
 */
void webSocketEvent(uint8_t num, WStype_t type, uint8_t * payload, size_t payLength){
  Serial.println("WEBSOCKET EVENT");
  Serial.println(millis()- timerPing);
  timerPing = millis();
  switch(type){
    case WStype_DISCONNECTED:{
      //perform disconnection events (i.e. send bot to idle.)
      IPAddress ip = webSocket.remoteIP(num);
      Serial.println(String(num) + " Client from " + ip[0] + "." + ip[1] + "." + ip[2] + "." + ip[3] + " Has Disconnected " + "\n");
      //Stop Bot
      Stop();
    }
      break;
    case WStype_CONNECTED:{
      //HANDLE NEW CLIENT CONNECTION
      IPAddress ip = webSocket.remoteIP(num);
      Serial.println(String(num) + " Connected from " + ip[0] + "." + ip[1] + "." + ip[2] + "." + ip[3] + " url: " + String(*payload) + "\n");
      //ACK connection to client
      webSocket.sendTXT(num, "Connected to FH_Tbot");
    }
    break;
      case WStype_TEXT:{
      //Perform actions based on a good payload.
      Serial.println("Starting charstream to char array conversion");
      char A[payLength + 1];
      char * strncpy(char * A, const char * payload, size_t payLength);
      A[payLength] = '\0';
      String b((char *) payload);
      Serial.println(String(num) + "get Text: " + b + " length: " + String(payLength) + "\n");
      //TEMP BOUNCE
      //webSocket.sendTXT(num, "Pong");
      //Heartbeat
      HeartBeatRcvd = true;
      fastExecuteRequest(b);
      //Feedback
    }
      break;
    }
  }

void setupWiFi()
{
  WiFi.mode(WIFI_AP);

  // Create a unique name by appending the MAC address to the AP Name

  AP_Name = AP_Name + " " + WiFi.softAPmacAddress();
  char AP_NameChar[AP_Name.length() + 1];
  AP_Name.toCharArray(AP_NameChar,AP_Name.length() + 1);

  /*
   * struct softap_config {

// D4 is used for neoPixelBus (TXD1)
// D0 is used to trigger ping
// D8 is used for echo of ping
#define D9 3 // D9 & D10 aren't defined so define them here
#define D10 1
const int motorLeftA = D6;
const int motorLeftB = D5;
const int motorRightA = D2;
const int motorRightB = D3;
const int motorLeftEncoder = D7;
