No Kid Left Behind

//Master Location Dashboard. 

//The program controls up to 20 devices internally and 15 devices on screen
//It uses the SD card to log data into two different files. 
//DataRSSI file contains the RSSI values of the AP devices in the ESPNow channel.
//DataPing file contains the packet latency akcnowledgement time for each one of the data packages sent.
//It keeps track of all the devices connected by verifying that their SSID format matches the costum string.
//The SSID is formatted as TOURNAME:COSTUMERNAME:MAC_ADDRESS. Where TOURNAME is no longer than 6 characters, 
//COSTUMERNAME is up to 5 characters and the MAC_ADDRESS is a six touple value.
//The graphical interface supports touch screen input and displays up to 15 devices. It allows the user to select one 
//device and display MAC address, costumer name assigned and tour name.
//Finally, the program alerts in case any of the devices losing connection and shows the costumer name it was assigned to.

//NOTE: Performanace improvement remove all the serial print commands.

//This program uses as a basic template the master.ino program from the ESP32\EspNow 

#include <SPI.h>
#include <Wire.h>      
#include <SD.h>

#include <Adafruit_GFX.h>         // Core graphics library
#include <Adafruit_ILI9341.h>     // Hardware-specific library
#include <Adafruit_STMPE610.h>

//ESPNow library
#include <esp_now.h>
#include <WiFi.h>

//Tour name. Sample: WAMALL --> Washington Mall
#define TOURNAME "WAMALL"

// Pin definitions for 2.4" TFT FeatherWing vary among boards...
//We could have got away with just the ESP32, but we could have used a ESP8266. Left for future use.
#if defined(ESP8266)
  #define TFT_CS   0
  #define TFT_DC   15
  #define SD_CS    2
#elif defined(ESP32)
  #define TFT_CS   15
  #define TFT_DC   33
  #define SD_CS    14
#endif

#ifdef ESP32
   #define STMPE_CS 32
   #define TFT_CS   15
   #define TFT_DC   33
   #define SD_CS    14
#endif

// This is calibration data for the raw touch data to the screen coordinates
#define TS_MINX 3800
#define TS_MAXX 100
#define TS_MINY 100
#define TS_MAXY 3750

#define CHANNEL 4
#define PRINTSCANRESULTS 0

// Global copy of slave
#define NUMSLAVES 20
//Device distance range status constants
#define STATUS_HERE     0
#define STATUS_LOST     1
#define STATUS_CLOSE    2
#define STATUS_FAR      3

//Tailored structure to keep track of the connected devices.
typedef struct nodeData{
  String  nodeSSID;
  String  tourName;
  String  costumerName;
  int     Status;
  int     Current_RSSI;
};

//Filename definitions and constants
#define UNIQUE_ID_FILENAME "/FileNameId.txt"
#define FILE_RSSI "/DataRSSI_"
#define FILE_PING "/DataPing_"
#define FILE_EXT  ".vis"

bool openSDFlg=false;
bool openRSSIFlg=false;
bool openPingFlg=false;
File fnCounter;
File dataRSSI;
File dataPing;
String fnPing;
String fnRSSI;
int fnCounterId =0; //Automatic incremental file id to generate unique files every time it runs

bool PanicFlag=false; //Alert a device(s) have lost connection
bool ClearDisFlag=false;
bool NeedRefresh = false;
//Costum array data for each device connected
nodeData peerNodeData[NUMSLAVES];

//Internal device connection
esp_now_peer_info_t slaves[NUMSLAVES] = {};
int checkConCounter[NUMSLAVES];
int SlaveCnt = 0;
int PrevSel = 0;

//Instantiation of the display and touch screen interface objects.
Adafruit_ILI9341  tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
Adafruit_STMPE610 ts  = Adafruit_STMPE610(STMPE_CS);
//Timing variables
unsigned long start =0;
unsigned long startPeer[NUMSLAVES];
unsigned long refreshTime =0;
//Variables that defines the display dimensions.
int rectHeight;
int rectWidth;
int radius;
//Define constants for device status
#define STATUS_UNKNOWN  0
#define STATUS_OK       1
#define STATUS_WARNING  2
#define STATUS_DANGER   3
//Define constants for radio frequency strenght
#define RANGE_OK        45
#define RANGE_WARNING   69
#define RANGE_DANGER    70

//More display constants to grid the screen
#define DISCON_MAX  5
#define GRID_MAX_X    5
#define GRID_MAX_Y    3
#define USE_SCREEN_Y    1 
#define USE_SCREEN_DIV  2 
//Amount of time to rescan the network
#define REFRESH_THRESHOLD 3000000
//More display variables
int OffsetX=0;
int OffsetY=0;
//Debug control flags. Did not work that well.
#define DEBUG_ESPNOW  1
//#define DEBUG_TS      1
//Device selected flag.
int FlgSelect = 0;

//Function that initializes the file logging.
//Opens an index file that contains the sequential unique counter to be attached to the login filename, to create a unique filename each time the device runs.
void InitLog()
{
  if(!SD.begin(SD_CS)) 
  {
    Serial.println(F("failed!"));
    openSDFlg = false;
  }
  else
  {
    uint8_t cardType = SD.cardType();
    Serial.println("SD OK");
    if(cardType == CARD_NONE){
        Serial.println("No SD card attached");
        openSDFlg = false;
    }
    else
    {
      static uint8_t buf[512];
      Serial.println("Check file Exists");
      if (SD.exists(UNIQUE_ID_FILENAME))
      {
        Serial.println("File Exists");
        fnCounter = SD.open(UNIQUE_ID_FILENAME);
        if (fnCounter)
        {
          Serial.println("Open File and read");
          Serial.println(fnCounter.size());
          fnCounter.read(buf,fnCounter.size());
          Serial.println(String((char *)buf));
          fnCounterId = String((char *)buf).toInt();
          Serial.print("Counter: ");
          Serial.println(fnCounterId);
          fnCounter.close();
          fnCounter=SD.open(UNIQUE_ID_FILENAME,FILE_WRITE);
          fnCounter.print(String(fnCounterId+1));
          fnCounter.close();
        }
      }
      else
      {
        Serial.println("Create File");
        fnCounter = SD.open(UNIQUE_ID_FILENAME,FILE_WRITE);
        if (fnCounter)
        {
          Serial.println("File open for write");
          fnCounter.print("0");
          fnCounter.close();
        }
        else
          Serial.println("Error");
        fnCounterId =0;
      }

      String tmpStr = FILE_RSSI;
      tmpStr.concat(String(fnCounterId));
      tmpStr.concat(FILE_EXT);
      fnRSSI = tmpStr;
      dataRSSI = SD.open(tmpStr,FILE_APPEND);
      if (dataRSSI)
      {
        Serial.println("RSSI ready");
        openRSSIFlg = true;
        //dataRSSI.print("THIs is atest");
        dataRSSI.flush();
      }

      tmpStr = FILE_PING;
      tmpStr.concat(String(fnCounterId));
      tmpStr.concat(FILE_EXT);
      fnPing = tmpStr;
      dataPing = SD.open(tmpStr,FILE_APPEND);
      if (dataPing)
      {
        Serial.print(tmpStr);
        Serial.println("Ping ready");
        openPingFlg = true;
        //dataPing.print("THIs is atest");
        dataPing.flush();
      }
      
      openSDFlg = true;
    }
  }
  
}

//This function cycles the close/open process of the logging files.
//This function is needed due to the flush function now working. The function is call every 3 seconds and dumps the buffer to the files.
void RefreshFiles()
{
    dataRSSI.close();
    dataRSSI = SD.open(fnRSSI,FILE_APPEND);
    if (dataRSSI)
    {
      Serial.println("RSSI ready");
      openRSSIFlg = true;
      dataRSSI.flush();
    }
    else
      openPingFlg = false;

    dataPing.close();
    dataPing = SD.open(fnPing,FILE_APPEND);
    if (dataPing)
    {
      Serial.println("Ping ready");
      openPingFlg = true;
      dataPing.flush();
    }
    else
      openPingFlg = false;
}

//This function updates the status of the display interface and logs the changes per device.
void UpdateStatus(int Pos,int numTotX,int Status,int offsetPos,bool refreshFile)
{
  int PosX = Pos%numTotX;
  int PosY = Pos/numTotX;
  int startY = rectHeight * offsetPos;
  String tmpStr;
  
  switch (Status)
  {
    case STATUS_UNKNOWN:
        tft.fillCircle((PosX*rectWidth)+rectWidth/2,startY + (PosY*rectHeight)+rectHeight/2,radius,ILI9341_WHITE);
        peerNodeData[Pos].Status = STATUS_LOST;
        if (openRSSIFlg && refreshFile)
        {
          tmpStr = "Status : ALERT LOST "+peerNodeData[Pos].costumerName+" "+peerNodeData[Pos].nodeSSID+" "+String(micros()/1000);
          dataRSSI.println(tmpStr);
          dataRSSI.flush();
          Serial.print(tmpStr);
        }
      break;
    case STATUS_OK:
        tft.fillCircle((PosX*rectWidth)+rectWidth/2,startY + (PosY*rectHeight)+rectHeight/2,radius,ILI9341_GREEN);
        peerNodeData[Pos].Status = STATUS_HERE;
        if (openRSSIFlg && refreshFile)
        {
          tmpStr = "Status : Here "+peerNodeData[Pos].costumerName+" "+peerNodeData[Pos].nodeSSID+" "+String(peerNodeData[Pos].Current_RSSI)+" "+String(micros()/1000);
          dataRSSI.println(tmpStr);
          dataRSSI.flush();
          Serial.print(tmpStr);
        }
      break;
    case STATUS_WARNING:
        tft.fillCircle((PosX*rectWidth)+rectWidth/2,startY + (PosY*rectHeight)+rectHeight/2,radius,ILI9341_YELLOW);
        peerNodeData[Pos].Status = STATUS_CLOSE;
        if (openRSSIFlg && refreshFile)
        {
          tmpStr = "Status : Close "+peerNodeData[Pos].costumerName+" "+peerNodeData[Pos].nodeSSID+" "+String(peerNodeData[Pos].Current_RSSI)+" "+String(micros()/1000);
          dataRSSI.println(tmpStr);
          dataRSSI.flush();
          Serial.print(tmpStr);
        }
      break;
    case STATUS_DANGER:
        tft.fillCircle((PosX*rectWidth)+rectWidth/2,startY + (PosY*rectHeight)+rectHeight/2,radius,ILI9341_RED);
        peerNodeData[Pos].Status = STATUS_FAR;
        if (openRSSIFlg && refreshFile)
        {
          tmpStr = "Status : Far "+peerNodeData[Pos].costumerName+" "+peerNodeData[Pos].nodeSSID+" "+String(peerNodeData[Pos].Current_RSSI)+" "+String(micros()/1000);
          dataRSSI.println(tmpStr);
          dataRSSI.flush();
          Serial.print(tmpStr);
        }
      break;
  }
  tft.fillCircle((PosX*rectWidth)+rectWidth/2,startY + (PosY*rectHeight)+rectHeight/2,radius-4,ILI9341_BLACK);
}

//This function selects the node on the display according to the touch screen cordinates passed.
//The function tries to flush all the commands in the touch screen stack, before returning.
void SelectNode(TS_Point posScreen,int offsetPos)
{
  int posX;
  int posY;
  int pos;
  TS_Point p;
  int startY = rectHeight * offsetPos;

  posX = posScreen.x /rectWidth ;
  posY = posScreen.y /rectHeight ;
  pos = posX + (posY*GRID_MAX_X);

#ifdef DEBUG_TS
  Serial.print("X = "); Serial.print(posScreen.x);
  Serial.print("\tY = "); Serial.println(posScreen.y);
  Serial.print("PX = "); Serial.print(posX);
  Serial.print("\tPY = "); Serial.println(posY);
  Serial.print("Pos = "); Serial.print(pos);
  Serial.print("\tPrev = "); Serial.println(PrevSel);
#endif

  if ((pos >= offsetPos && pos < offsetPos+SlaveCnt) && pos != PrevSel)
  {
     
    tft.drawRect((PrevSel%GRID_MAX_X)*rectWidth,startY+(PrevSel/GRID_MAX_X)*rectHeight,rectWidth,rectHeight,ILI9341_WHITE);
    tft.drawRect(posX*rectWidth,startY+(posY*rectHeight),rectWidth,rectHeight,ILI9341_BLUE);
    PrevSel = pos;
  }

  if (FlgSelect > 1 && (posScreen.x > tft.width()-rectWidth && posScreen.y > tft.height()-rectHeight)) 
  {
    FlgSelect=0;
    tft.fillRect(tft.width()-rectWidth,startY+(tft.height()-rectHeight), rectWidth,rectHeight, ILI9341_BLACK);
    tft.drawRect(tft.width()-rectWidth,startY+(tft.height()-rectHeight),rectWidth,rectHeight,ILI9341_CYAN);
    ClearScreen();
    CreateGrid(GRID_MAX_X,GRID_MAX_Y,0);
    CreateGrid(GRID_MAX_X,GRID_MAX_Y,3);
  }
  else
    FlgSelect++;

  while (ts.touched())
  {
    p = ts.getPoint();
  }
    //delay(100);
  while (!ts.bufferEmpty())
  {
    p = ts.getPoint();
  }
}

//Creates the grid on the display according to the number of bins to supper per row and column
void CreateGrid(int numNodesHorz, int numNodesVert,int offsetPos)
{
  int i,j;
  int cx = USE_SCREEN_Y*(tft.height()/USE_SCREEN_DIV)-1;
  int startY=0;

  rectHeight = cx / numNodesVert;
  rectWidth = tft.width() / numNodesHorz;
  radius = (((rectHeight < rectWidth)? rectHeight: rectWidth)/2)-2;
  startY = rectHeight * offsetPos;

  for(i=0;i<numNodesVert;i++)
  {
    for (j=0;j<numNodesHorz;j++)
    {
      tft.drawRect(j*rectWidth,startY+(i*rectHeight),rectWidth,rectHeight,ILI9341_WHITE);
      tft.fillCircle((j*rectWidth)+rectWidth/2,startY+((i*rectHeight)+rectHeight/2),radius,ILI9341_WHITE);
      tft.fillCircle((j*rectWidth)+rectWidth/2,startY+((i*rectHeight)+rectHeight/2),radius-4,ILI9341_BLACK);
    }
  }

  tft.drawRect(tft.width()-rectWidth,tft.height()-rectHeight,rectWidth,rectHeight,ILI9341_CYAN);
  
}

//This function clear the screen and sets the default values for text.
void ClearScreen()
{
  tft.fillScreen(ILI9341_BLACK);
  tft.setCursor(0, 2);
  tft.setTextColor(ILI9341_WHITE,ILI9341_BLACK);  
  tft.setTextSize(2);  
}

//Refresh the status of the cube, depending on the RSSI value passed.
void RefreshCube(int Pos,int Value,int offsetPos, bool refreshFile)
{
   if (Value <= RANGE_OK)
      UpdateStatus(Pos,GRID_MAX_X,STATUS_OK,offsetPos,refreshFile);
   else if (Value <= RANGE_WARNING)
      UpdateStatus(Pos,GRID_MAX_X,STATUS_WARNING, offsetPos,refreshFile);
   else if (Value >= RANGE_DANGER)
      UpdateStatus(Pos,GRID_MAX_X,STATUS_DANGER,offsetPos,refreshFile);
   else
      UpdateStatus(Pos,GRID_MAX_X,STATUS_UNKNOWN,offsetPos,refreshFile);
}

//This function runs over the array of devices and check the number of consecutive attempts to connect to the device that went unsuccesfull
void ConfirmConnection(int offsetPos)
{
  bool foundDisconnect=false;
  for(int i=0;i<SlaveCnt;i++)
  {
     if (checkConCounter[i] >  DISCON_MAX)
     {
        UpdateStatus(i,GRID_MAX_X,STATUS_UNKNOWN,offsetPos,false);
        PanicFlag = true;
        foundDisconnect = true;
     }
  }

  if (!foundDisconnect)
  {
    PanicFlag= false;
    ClearDisFlag=false;
    if (NeedRefresh)
    {
      NeedRefresh=false;
      ClearScreen();
      CreateGrid(GRID_MAX_X,GRID_MAX_Y,0);
      CreateGrid(GRID_MAX_X,GRID_MAX_Y,3);
    }
  }
  else 
  {
    String tmpStr="Lost: ";
    bool FirstTime=true;

    tft.fillRect(0,tft.height()/2, tft.width(),tft.height()/2, ILI9341_BLACK);    
    tft.setTextColor(ILI9341_RED);  
    tft.setTextSize(3);
    printDisplay(15,"ALERT",0);
    tft.setTextColor(ILI9341_WHITE);  
    tft.setTextSize(2);
    for (int i=0;i<SlaveCnt;i++)
    {
      if (peerNodeData[i].Status == STATUS_LOST)
      {
        if (!FirstTime)
        {
          tmpStr.concat(",");
        }
        FirstTime=false;
        tmpStr.concat(peerNodeData[i].costumerName);
      }
    }
    printDisplay(20,tmpStr,0);    
    tft.setTextColor(ILI9341_WHITE);  
    tft.setTextSize(2);
    delay(200);

    if (!ClearDisFlag)
    {
      ClearScreen();
      CreateGrid(GRID_MAX_X,GRID_MAX_Y,0);
      ClearDisFlag=true;
      NeedRefresh=true;
    }
  }
}

//Initialize ESPNow protocol
void InitESPNow() 
{
  WiFi.disconnect();
  if (esp_now_init() == ESP_OK) {
    Serial.println("ESPNow Init Success");
  }
  else {
    Serial.println("ESPNow Init Failed");
    ESP.restart();
  }
}

//This function searches in the internal array if the peer is already register
int searchPeer(int peerMAC[])
{
  int retValue=-1;
  
  if (SlaveCnt ==0)
    retValue=-1;
  else
  {
    int i,j;
    bool FlgDifferent = false;

    for (i=0;i<SlaveCnt;i++)
    {
      FlgDifferent = false;
      for (j=0;j<6;j++)
      {
        if (peerMAC[j] != slaves[i].peer_addr[j])
        {
          FlgDifferent = true;
          break;
        }
      }

      if (!FlgDifferent)
      {
        retValue = i;
        break;
      }
        
    }
  }

  return retValue;
}

//This function performs a scan network searching for the devices that match the SSID format. 
//If found they are included in the register peers array.
void ScanForSlave() {
  int8_t scanResults = WiFi.scanNetworks(false,false,false,500);
  if (scanResults == 0) {
#ifdef DEBUG_ESPNOW
    Serial.println("No WiFi devices in AP Mode found");
#endif    
  } else {
#ifdef DEBUG_ESPNOW
    Serial.print("Found "); Serial.print(scanResults); Serial.println(" devices ");
#endif    
    for (int i = 0; i < scanResults; ++i) {
      // Print SSID and RSSI for each device found
      String SSID = WiFi.SSID(i);
      int32_t RSSI = WiFi.RSSI(i);
      String BSSIDstr = WiFi.BSSIDstr(i);

#ifdef DEBUG_ESPNOW
      if (PRINTSCANRESULTS) {
        Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
      }
#endif      
      if (SSID.indexOf(TOURNAME) == 0) {
        int mac[6];
        int CurPos;
        int numMacElem=0;

        numMacElem = sscanf(BSSIDstr.c_str(), "%x:%x:%x:%x:%x:%x%c",  &mac[0], &mac[1], &mac[2], &mac[3], &mac[4], &mac[5] );
        CurPos = searchPeer(mac);
        if (CurPos < 0)
        {
            // SSID of interest
#ifdef DEBUG_ESPNOW
            Serial.print(i + 1); Serial.print(": "); Serial.print(SSID); Serial.print(" ["); Serial.print(BSSIDstr); Serial.print("]"); Serial.print(" ("); Serial.print(RSSI); Serial.print(")"); Serial.println("");
#endif            
            // Get BSSID => Mac Address of the Slave
    
            if ( 6 == numMacElem ) {
              for (int ii = 0; ii < 6; ++ii ) {
                slaves[SlaveCnt].peer_addr[ii] = (uint8_t) mac[ii];
              }
            }
            slaves[SlaveCnt].channel = CHANNEL; // pick a channel
            slaves[SlaveCnt].encrypt = 0; // no encryption
            if ((SlaveCnt) == 0)
              tft.setCursor(12+(rectWidth*(SlaveCnt%GRID_MAX_X)),((rectHeight/2)*(SlaveCnt/GRID_MAX_X))+20); //(tft.height()/2) + 
            else
              tft.setCursor(12+(rectWidth*(SlaveCnt%GRID_MAX_X)),((rectHeight)*(SlaveCnt/GRID_MAX_X))+20); //(tft.height()/2) + 
            RefreshCube(SlaveCnt,-1*RSSI,0,true);
            tft.print(-1*RSSI);
            checkConCounter[SlaveCnt] = 0;
            int FirstInstance = SSID.indexOf(":");
            int SecondInstance = SSID.indexOf(":",FirstInstance+1);
            peerNodeData[SlaveCnt].nodeSSID = SSID.substring(SecondInstance+1);
            peerNodeData[SlaveCnt].tourName = SSID.substring(0,FirstInstance);
            peerNodeData[SlaveCnt].costumerName = SSID.substring(FirstInstance+1,SSID.indexOf(":",FirstInstance+1));
            peerNodeData[SlaveCnt].Status = STATUS_HERE;
            peerNodeData[SlaveCnt].Current_RSSI = RSSI;
            SlaveCnt++;
        }
        else
        {
            if ((CurPos) == 0)
              tft.setCursor(12+(rectWidth*(CurPos%GRID_MAX_X)),((rectHeight/2)*(CurPos/GRID_MAX_X))+20); 
            else
              tft.setCursor(12+(rectWidth*(CurPos%GRID_MAX_X)),((rectHeight)*(CurPos/GRID_MAX_X))+20); 
            peerNodeData[CurPos].Current_RSSI = RSSI;
            RefreshCube(CurPos,-1*RSSI,0,true);
            tft.print(-1*RSSI);
            checkConCounter[CurPos] = 0;
        }
      }
    }

    for(int i=0;i<SlaveCnt;i++)
      checkConCounter[i]++;
  }

#ifdef DEBUG_ESPNOW
  if (SlaveCnt > 0) {
    Serial.print(SlaveCnt); Serial.println(" Slave(s) found, processing..");
  } else {
    Serial.println("No Slave Found, trying again.");
  }
#endif

  // clean up ram
  WiFi.scanDelete();
}

// Check if the slave is already paired with the master.
// If not, pair the slave with master
void manageSlave() {
  if (SlaveCnt > 0) {
    for (int i = 0; i < SlaveCnt; i++) {
      const esp_now_peer_info_t *peer = &slaves[i];
      const uint8_t *peer_addr = slaves[i].peer_addr;
      Serial.print("Processing: ");
      for (int ii = 0; ii < 6; ++ii ) {
        Serial.print((uint8_t) slaves[i].peer_addr[ii], HEX);
        if (ii != 5) Serial.print(":");
      }
      Serial.print(" Status: ");
      // check if the peer exists
      bool exists = esp_now_is_peer_exist(peer_addr);
      if (exists) {
        // Slave already paired.
        Serial.println("Already Paired");
      } else {
        // Slave not paired, attempt pair
        esp_err_t addStatus = esp_now_add_peer(peer);
        if (addStatus == ESP_OK) {
          // Pair success
          Serial.println("Pair success");
        } else if (addStatus == ESP_ERR_ESPNOW_NOT_INIT) {
          // How did we get so far!!
          Serial.println("ESPNOW Not Init");
        } else if (addStatus == ESP_ERR_ESPNOW_ARG) {
          Serial.println("Add Peer - Invalid Argument");
        } else if (addStatus == ESP_ERR_ESPNOW_FULL) {
          Serial.println("Peer list full");
        } else if (addStatus == ESP_ERR_ESPNOW_NO_MEM) {
          Serial.println("Out of memory");
        } else if (addStatus == ESP_ERR_ESPNOW_EXIST) {
          Serial.println("Peer Exists");
        } else {
          Serial.println("Not sure what happened");
        }
      }
    }
  } else {
    // No slave found to process
#ifdef DEBUG_ESPNOW
    Serial.println("No Slave found to process");
#endif    
  }
}


uint8_t data = 0;
// send data
void sendData() {
  data++;
  for (int i = 0; i < SlaveCnt; i++) {
    const uint8_t *peer_addr = slaves[i].peer_addr;

#ifdef DEBUG_ESPNOW
    if (i == 0) { // print only for first slave
      Serial.print("Sending: ");
      Serial.println(data);
    }
#endif
    
    esp_err_t result = esp_now_send(peer_addr, &data, sizeof(data));
    Serial.print("Send Status: ");
    if (result == ESP_OK) {
      Serial.println("Success");
    } else if (result == ESP_ERR_ESPNOW_NOT_INIT) {
      // How did we get so far!!
      Serial.println("ESPNOW not Init.");
    } else if (result == ESP_ERR_ESPNOW_ARG) {
      Serial.println("Invalid Argument");
    } else if (result == ESP_ERR_ESPNOW_INTERNAL) {
      Serial.println("Internal Error");
    } else if (result == ESP_ERR_ESPNOW_NO_MEM) {
      Serial.println("ESP_ERR_ESPNOW_NO_MEM");
    } else if (result == ESP_ERR_ESPNOW_NOT_FOUND) {
      Serial.println("Peer not found.");
    } else {
      Serial.println("Not sure what happened");
    }
    startPeer[i] = micros();
  }
}

//Display at the specified position the meesage passed. 
void printDisplay(int iPos,String strData,int offsetPos)
{
    int startY = rectHeight * offsetPos;

    if ((iPos) == 0)
      tft.setCursor(12+(rectWidth*(iPos%GRID_MAX_X)),startY+((rectHeight/2)*(iPos/GRID_MAX_X))+20); 
    else
      tft.setCursor(12+(rectWidth*(iPos%GRID_MAX_X)),startY+((rectHeight)*(iPos/GRID_MAX_X))+20); 
    tft.print(strData);
}

// callback when data is sent from Master to Slave
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
  char macStr[18];
  int CurPos;
  int mac[6];
  
  snprintf(macStr, sizeof(macStr), "%02x:%02x:%02x:%02x:%02x:%02x",
           mac_addr[0], mac_addr[1], mac_addr[2], mac_addr[3], mac_addr[4], mac_addr[5]);

  for (int i=0;i<6;i++)
    mac[i] = mac_addr[i];
  CurPos = searchPeer(mac);
  unsigned long tmpTime =(micros()-startPeer[CurPos])/1000;

  if (status == ESP_NOW_SEND_SUCCESS && !PanicFlag)
  {
    RefreshCube(15+CurPos,0,0,false);
    printDisplay(15+CurPos,String(tmpTime),0);
    if (openPingFlg)
    {
      String tmpStr = "Received OK "+peerNodeData[CurPos].costumerName+" "+peerNodeData[CurPos].nodeSSID+" "+String(peerNodeData[CurPos].Current_RSSI)+" "+String(tmpTime);
      dataPing.println(tmpStr);
      dataPing.flush();
      Serial.print(tmpStr);
    }
  }
  else
  {
    if (status != ESP_NOW_SEND_SUCCESS)
    {
      if (openPingFlg)
      {
        String tmpStr = "Received Failed "+peerNodeData[CurPos].costumerName+" "+peerNodeData[CurPos].nodeSSID+" "+String(peerNodeData[CurPos].Current_RSSI)+" "+String(tmpTime);
        dataPing.println(tmpStr);
        dataPing.flush();
        Serial.print(tmpStr);
      }
    }
    
  }
  
#ifdef DEBUG_ESPNOW
  Serial.print("Last Packet Sent to: "); Serial.println(macStr);
  Serial.print("Last Packet Send Status: "); Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
#endif  
  
}

//Initialize all the components.
void setup() {
  memset(slaves, 0, sizeof(slaves));
  memset(checkConCounter, 0, sizeof(checkConCounter));
  Serial.begin(115200);
  //Set device in STA mode to begin with
  WiFi.mode(WIFI_STA);
#ifdef DEBUG_ESPNOW
  Serial.println("ESPNow/Multi-Slave/Master Example");
  Serial.print("STA MAC: "); Serial.println(WiFi.macAddress());
#endif  
  InitESPNow();
  esp_now_register_send_cb(OnDataSent);

  ts.begin();
  tft.begin();          // Initialize screen

  ClearScreen();
  CreateGrid(GRID_MAX_X,GRID_MAX_Y,0);
  CreateGrid(GRID_MAX_X,GRID_MAX_Y,3);

  ScanForSlave();

  refreshTime = micros();

  InitLog();
}

int loopCnt=0;

//Main loop
void loop() {

  if (!FlgSelect)
  {
    // wait for 3seconds to run the logic again
    if (micros()-refreshTime > REFRESH_THRESHOLD)
    {
      ScanForSlave();
      refreshTime = micros();
      RefreshFiles();
    }
    
    if (SlaveCnt > 0) { 
      manageSlave();
      sendData();
    } 
  }
 
  ConfirmConnection(0);

  if (ts.touched())
  {
    TS_Point p = ts.getPoint();
    
    while (!ts.bufferEmpty())
    {
      p = ts.getPoint();
    }

    delay(100);

    p = ts.getPoint();
    p.x = map(p.x, TS_MINX, TS_MAXX, 0, tft.width());
    p.y = map(p.y, TS_MINY, TS_MAXY, 0, tft.height());

    if (FlgSelect)
    {
      p = ts.getPoint();
      p.x = map(p.x, TS_MINX, TS_MAXX, 0, tft.width());
      p.y = map(p.y, TS_MINY, TS_MAXY, 0, tft.height());
      SelectNode(p,0);
      if (FlgSelect)
      {
        ClearScreen();
        delay(100);
        tft.fillRect(tft.width()-rectWidth,tft.height()-rectHeight, rectWidth,rectHeight, ILI9341_CYAN);
        String strTempo = "MAC ";
        strTempo.concat(peerNodeData[PrevSel].nodeSSID); 
        printDisplay(0,strTempo,0);
        strTempo = "Name: ";
        strTempo.concat(peerNodeData[PrevSel].costumerName); 
        printDisplay(5,strTempo,0);
        strTempo = "Tour: ";
        strTempo.concat(peerNodeData[PrevSel].tourName); 
        printDisplay(10,strTempo,0);
        
      }
    }
    else
    {
 
      if (p.x > tft.width()-rectWidth && p.y > tft.height()-rectHeight) 
      {
        if (FlgSelect == 0)
        {
          FlgSelect++;
          tft.fillRect(tft.width()-rectWidth,tft.height()-rectHeight, rectWidth,rectHeight, ILI9341_CYAN);
  
          while (ts.touched())
          {
            p = ts.getPoint();
          }
            //delay(100);
          while (!ts.bufferEmpty())
          {
            p = ts.getPoint();
          }
        }
      }
    }
  
  }

}