#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#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>

#define SDA_PIN 21
#define SCL_PIN 22

const char* ssid = "BUS";
int busRSSI = 0;
int stop2RSSI = 0;
int stop1RSSI = 0;


Adafruit_MPU6050 mpu;
esp_now_peer_info_t peerInfo1;
esp_now_peer_info_t peerInfo2;
// REPLACE WITH YOUR RECEIVER MAC Address
uint8_t busStop2Address[] = {0x30, 0xC9, 0x22, 0x28, 0x4E, 0xCC};
uint8_t busStop1Address[] = {0xEC, 0x64, 0xC9, 0x85, 0x58, 0x68}; // mac address for Bus Stop 1


void setup(void) {
   Serial.begin(115200);
  Wire.begin(SDA_PIN, SCL_PIN);
 
  // 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(peerInfo1.peer_addr, busStop2Address, 6);
  peerInfo1.channel = 0;  
  peerInfo1.encrypt = false;
  
  memcpy(peerInfo2.peer_addr, busStop1Address, 6);
  peerInfo2.channel = 0;  
  peerInfo2.encrypt = false;
  
  // Add peer        
  if (esp_now_add_peer(&peerInfo1) != ESP_OK){
    Serial.println("Failed to add bus stop 2 peer");
    return;
  }
  if (esp_now_add_peer(&peerInfo2) != ESP_OK){
    Serial.println("Failed to add bus stop 1 peer");
    return;
  }

  //Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  Serial.println("1");
  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  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);
}


bool busreceive = false;               // Needs to be set true if you want to receive else set false.
bool send_to_stop2 = true;              // Needs to set true if you want to send data to stop2 else false
bool send_to_stop1 = true;                // Needs to be true if you want to send data to bus else false

bool move = true;
bool toa = 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 a[32];
  int b;
  float c;
  bool d;
} struct_message;

// Create a struct_message called myData
struct_message myData;


// 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 Bus Success" : "Delivery To Bus 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(&myData, incomingData, sizeof(myData));
  Serial.print("Bytes received: ");
  Serial.println(len);
  Serial.print("Char: ");
  Serial.println(myData.a);
  Serial.print("Int: ");
  Serial.println(myData.b);
  Serial.print("Float: ");
  Serial.println(myData.c);
  Serial.print("Bool: ");
  Serial.println(myData.d);
  Serial.println();
}


void loop() {   // isMove function
  /* Get new sensor events with the readings */

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  /* Print out the values */
  int b = 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)){
      b++;
    }
    delay(100);
  }  
  // Serial.print(" ,");
  if(b >= 10){
    Serial.print(0);
    move = false;
    Serial.println(move);
    if (!toa) {
    //send_to_stop2 == true;
      strcpy(myData.a, "bus");
      myData.b = 100;
      myData.c = 1.2698;
      myData.d = move;
      esp_now_register_send_cb(OnDataSent2);
    
      // Send message via ESP-NOW
      esp_err_t result = esp_now_send(busStop2Address, (uint8_t *) &myData, sizeof(myData));          // 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");
      }
    }
    else if (toa){
      strcpy(myData.a, "bus");
      myData.b = 100;
      myData.c = 1.2698;
      myData.d = move;
      esp_now_register_send_cb(OnDataSent1);
    
      // Send message via ESP-NOW
      esp_err_t result = esp_now_send(busStop1Address, (uint8_t *) &myData, sizeof(myData));          // 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");
      }
    }
      //b = 0;
    }
  else if(b < 10){
    Serial.print(1);
    move = true;
    if (!toa) {
    //send_to_stop2 == true;
      strcpy(myData.a, "bus");
      myData.b = 100;
      myData.c = 1.2698;
      myData.d = move;
      esp_now_register_send_cb(OnDataSent2);
    
      // Send message via ESP-NOW
      esp_err_t result = esp_now_send(busStop2Address, (uint8_t *) &myData, sizeof(myData));          // 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");
      }
    }
    else if (toa){
      strcpy(myData.a, "bus");
      myData.b = 100;
      myData.c = 1.2698;
      myData.d = move;
      esp_now_register_send_cb(OnDataSent1);
    
      // Send message via ESP-NOW
      esp_err_t result = esp_now_send(busStop1Address, (uint8_t *) &myData, sizeof(myData));          // 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");
      }
    }
      //b = 0;

    //b = 1;
  }

  Serial.println("");
  delay(100);
  /*
  if(send_to_stop1 == true){
    // Set values to send to bus.
    strcpy(myData.a, "1");
    myData.b = 298;
    myData.c = 1.2698;
    myData.d = false;
    esp_now_register_send_cb(OnDataSent2);
  
    // Send message via ESP-NOW
    esp_err_t result = esp_now_send(busAddress, (uint8_t *) &myData, sizeof(myData));       // 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(busreceive){
    // Do not change this code
    esp_now_register_recv_cb(OnDataRecv);
    if (strcmp(myData.a, "1") ==0 || strcmp(myData.a, "2")==0){
      toa = !toa;
    }

  }
  
  
  //send_to_stop2 == false;
  //delay(500);
  delay(6000);          // This delay of 1 second is necessary otherwise you might send and receive erronius data.

  delay(1000);
}