#include <WiFiManager.h>
#include <strings_en.h>
#include <wm_consts_en.h>
#include <wm_strings_en.h>
#include <wm_strings_es.h>
#include <esp_now.h>
#include <WiFi.h>

const char* ssid = "BUS";
int iBusRSSI = 0;
int iStop2RSSI = 0;
int iStop1RSSI = 0;

// REPLACE WITH YOUR RECEIVER MAC Address
uint8_t abBusStop2Address[] = {0x30, 0xC9, 0x22, 0x28, 0x4E, 0xCC};
uint8_t abBusStop1Address[] = {0xEC, 0x64, 0xC9, 0x85, 0x58, 0x68};

bool bBusReceive = false;               // Needs to be set true if you want to receive else set false.
bool bSendToStop2 = true;              // Needs to set true if you want to send data to stop2 else false
bool bSendToStop1 = true;                // Needs to be true if you want to send data to stop1 else false

// Structure example to send data
// Must match the receiver structure
typedef struct struct_message {
  // Variable to store the data received or data to be sent
  char acData[32];
  int iValue;
  float fValue;
  bool bFlag;
} struct_message;

// Create a struct_message called myData
struct_message stMyData;

esp_now_peer_info_t stPeerInfo1;
esp_now_peer_info_t stPeerInfo2;

// callback when data is sent
void OnDataSent1(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery to BusStop2 Success" : "Delivery to BusStop2 Fail");
}

void OnDataSent2(const uint8_t *mac_addr, esp_now_send_status_t status) {
  Serial.print("\r\nLast Packet Send Status:\t");
  Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery To BusStop1 Success" : "Delivery To BusStop1 Fail");
}
 
void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
  // Any variable that requires to take the value received needs to be assigned here.
  memcpy(&stMyData, incomingData, sizeof(stMyData));
  Serial.print("Bytes received: ");
  Serial.println(len);
  Serial.print("Char: ");
  Serial.println(stMyData.acData);
  Serial.print("Int: ");
  Serial.println(stMyData.iValue);
  Serial.print("Float: ");
  Serial.println(stMyData.fValue);
  Serial.print("Bool: ");
  Serial.println(stMyData.bFlag);
  Serial.println();
}

void setup() {
  // Init Serial Monitor
  Serial.begin(115200);

  while (!Serial) {
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  }

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
    case MPU6050_RANGE_2_G:
      Serial.println("+-2G");
      break;
    case MPU6050_RANGE_4_G:
      Serial.println("+-4G");
      break;
    case MPU6050_RANGE_8_G:
      Serial.println("+-8G");
      break;
    case MPU6050_RANGE_16_G:
      Serial.println("+-16G");
      break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
    case MPU6050_BAND_260_HZ:
      Serial.println("260 Hz");
      break;
    case MPU6050_BAND_184_HZ:
      Serial.println("184 Hz");
      break;
    case MPU6050_BAND_94_HZ:
      Serial.println("94 Hz");
      break;
    case MPU6050_BAND_44_HZ:
      Serial.println("44 Hz");
      break;
    case MPU6050_BAND_21_HZ:
      Serial.println("21 Hz");
      break;
    case MPU6050_BAND_10_HZ:
      Serial.println("10 Hz");
      break;
    case MPU6050_BAND_5_HZ:
      Serial.println("5 Hz");
      break;
  }

  Serial.println("");
  delay(100);
 
  // Set device as a Wi-Fi Station
  WiFi.softAP(ssid, NULL);
  WiFi.mode(WIFI_AP);

  // Init ESP-NOW ( Do not modify these lines)
  if (esp_now_init() != ESP_OK) {
    Serial.println("Error initializing ESP-NOW");
    return;
  }

  // Once ESPNow is successfully Init, we will register for Send CB to
  // get the status of Trasnmitted packet
  
  // Register peer
  memcpy(stPeerInfo1.peer_addr, abBusStop2Address, 6);
  stPeerInfo1.channel = 0;  
  stPeerInfo1.encrypt = false;
  
  memcpy(stPeerInfo2.peer_addr, abBusStop1Address, 6);
  stPeerInfo2.channel = 0;  
  stPeerInfo2.encrypt = false;
  
  // Add peer        
  if (esp_now_add_peer(&stPeerInfo1) != ESP_OK){
    Serial.println("Failed to add bus stop 2 peer");
    return;
  }
  if (esp_now_add_peer(&stPeerInfo2) != ESP_OK){
    Serial.println("Failed to add bus stop 1 peer");
    return;
  }

  // Add any code in setup after this.
}

void WiFiScanning() {
  int n = WiFi.scanNetworks();
  Serial.println("scan done");
  if (n == 0) {
    Serial.println("no networks found");
  } else {
    Serial.print(n);
    Serial.println(" networks found");
    for (int i = 0; i < n; ++i) {
      // Print SSID and RSSI for each network found
      Serial.print(i + 1);
      Serial.print(": ");
      Serial.print(WiFi.SSID(i));
      Serial.print(" (");
      Serial.print(WiFi.RSSI(i));
      Serial.print(")");
      Serial.println((WiFi.encryptionType(i) == WIFI_AUTH_OPEN) ? " " : "*");
      delay(10);
      if (WiFi.SSID(i) == "BUS_STOP_2") {
        iStop2RSSI = WiFi.RSSI(i);
      }
      if (WiFi.SSID(i) == "BUS_STOP_1") {
        iStop1RSSI = WiFi.RSSI(i);
      }
      if (WiFi.SSID(i) == "BUS") {
        iBusRSSI = WiFi.RSSI(i);
      }
    }
  }
  Serial.println("");
}

bool bMove() {
  /* Get new sensor events with the readings */
  bool bMov;
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  int num = 0;
  for (int i = 0; i < 10; i++) {
    if ((abs(a.acceleration.x) < 0.4) && (abs(a.acceleration.y) < 0.7) && (abs(a.acceleration.z) < 10.5)) {
      num++;
    }
    delay(100);
  }  
  
  if (num >= 10) {
    Serial.print(0);
    bMov = false;
    num = 0;
  } else if (num < 10) {
    Serial.print(1);
    bMov = true;
    num = 0;
  }

  Serial.println("");
  delay(500);
  return bMov;
}
 
void loop() {
  //WiFiScanning();
  if (bSendToStop2 == true) {
    // Set values to send to stop 2. Can be done outside the if statement
    strcpy(stMyData.acData, "Hi! Aditya");
    stMyData.iValue = 100;
    stMyData.fValue = 1.2698;
    stMyData.bFlag = bMove();

    esp_now_register_send_cb(OnDataSent1);
  
    // Send message via ESP-NOW
    esp_err_t result = esp_now_send(abBusStop2Address, (uint8_t *) &stMyData, sizeof(stMyData));          // Do not change this line until you need to use different struct variable
    
    if (result == ESP_OK) {
      Serial.println("Sent with success");
    } else {
      Serial.println("Error sending the data");
    }
  }

  delay(1000);          // This delay of 1 second is necessary otherwise you might send and receive erroneous data.

  if (bSendToStop1 == true) {
    // Set values to send to stop 1.
    strcpy(stMyData.acData, "Hi! Ayush");
    stMyData.iValue = 298;
    stMyData.fValue = 1.2698;
    stMyData.bFlag = bMove();

    esp_now_register_send_cb(OnDataSent2);
  
    // Send message via ESP-NOW
    esp_err_t result = esp_now_send(abBusStop1Address, (uint8_t *) &stMyData, sizeof(stMyData));       // Do not change this line until you need to use different struct variable
    
    if (result == ESP_OK) {
      Serial.println("Sent with success");
    } else {
      Serial.println("Error sending the data");
    }
  }
  
  delay(1000);
  
  if (bBusReceive) {
    // Do not change this code
    esp_now_register_recv_cb(OnDataRecv);
  }
  
  delay(1000);
}