#include "FS.h"
#include "SD.h"
#include "SPI.h"
#include<esp_now.h>
#include<WiFi.h>
#include<Wire.h>
const int MPU=0x68;//Device address
float AccX, AccY, AccZ;
int16_t AccX_temp, AccY_temp, AccZ_temp;
String success;
uint8_t broadcastAddress[] = {0x24, 0x62, 0xAB, 0xF9, 0x24, 0x78}; // Receiver's mac address 24:62:AB:F9:24:78


typedef struct struct_message{
    float AccX;
    float AccY;
    float AccZ;
} struct_message;

struct_message MPU6050Readings;

void setup(){
    Wire.begin(); //To redefine the I2C pins: Wire.begin(SDA,SCL) or Wire.begin(SDA, SCL, Bus_Speed).
  // Wake up the sensor (reset)
    Wire.beginTransmission(MPU);
    Wire.write(0x6B);//Wake up the MPU chip
    Wire.write(0x00);
    Wire.endTransmission(true);
    Wire.beginTransmission(MPU);
    Wire.write(0x1C);//Talk to the ACCEL_CONFIG register (1C hex)
    Wire.write(0x08);//Set the register bits as 00001000 (+/- 4g full scale range)
    Wire.endTransmission(true);     
    WiFi.mode(WIFI_STA);
    // Init ESP-NOW
    if (esp_now_init() != ESP_OK) {
      return;
    }
    // Register peer
    esp_now_peer_info_t peerInfo;
    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
    peerInfo.channel = 0;
    peerInfo.encrypt = false;
    // Add peer      
    if (esp_now_add_peer(&peerInfo) != ESP_OK){
      return;
  }
}
void loop(){
    accRead();
    MPU6050Readings.AccX=AccX;
    MPU6050Readings.AccY=AccY;
    MPU6050Readings.AccZ=AccZ;
    esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &MPU6050Readings, sizeof(MPU6050Readings)); //Send the data. &MPU6050Readings is just a 8-bit character pointer to store the data
}
void accRead(){ // read the MPU data to ESP32
    Wire.beginTransmission(MPU);
    Wire.write(0x3B);//Start with register 0x3B
    Wire.endTransmission(false);
    Wire.requestFrom(MPU,6,true);// Read 6 registers total
    AccX_temp=Wire.read()<<8 | Wire.read();
    AccX=AccX_temp;
    AccY_temp=Wire.read()<<8 | Wire.read();
    AccY=AccY_temp;
    AccZ_temp=Wire.read()<<8 | Wire.read();
    AccZ=AccZ_temp;
    Wire.endTransmission(true);
}