#include <WiFi.h>
#include <FirebaseESP32.h>
#include <addons/TokenHelper.h>
#include <addons/RTDBHelper.h>
#include <Wire.h>
#include <MPU6050.h>

#define flexPin 34
unsigned long dataMillisInterval = 1000; // Send Data Every [1000 ms] = [1 Second]

// Define the I2C addresses of the MPU6050 sensors
const uint8_t MPU1_ADDRESS = 0x68;
const uint8_t MPU2_ADDRESS = 0x69;


#define WIFI_SSID "network"
#define WIFI_PASSWORD "123456789"
#define API_KEY "AIzaSyCcrTq6x7TGHXQL4qQ3E48zDsqkaXBJQtw"
#define USER_EMAIL "project12338@gmail.com"
#define USER_PASSWORD "123456789"
#define DATABASE_URL "kneetracking-default-rtdb.europe-west1.firebasedatabase.app/"
#define DATABASE_SECRET "AaATdr2hwU6v5KUAITOHyG7KGl3BfhdQMEC2XFUQ"

FirebaseData fbdo;
FirebaseAuth auth;
FirebaseConfig config;
unsigned long dataMillis = 0;
MPU6050 mpu1(MPU1_ADDRESS);
MPU6050 mpu2(MPU2_ADDRESS);
double x_1, y_1, z_1;
double x2, y2, z2;
int analogValue;

void setup()
{
  Serial.begin(115200);
  setup_Sensors();
  connect_To_WiFi_and_Firebase();
}

void loop() {

  read_Flex_Sensor();
  read_MPU6050_Sensors();
  send_Data_To_Firebase();

}
//----------------------------------------------------------------
void send_Data_To_Firebase() {
  if (Firebase.ready() && (millis() - dataMillis > dataMillisInterval))
  {
    dataMillis = millis();
    Firebase.setIntAsync(fbdo, "/test/RAW_flex" , analogValue );
    Firebase.setIntAsync(fbdo, "/test/RAW_x_1"  , x_1         );
    Firebase.setIntAsync(fbdo, "/test/RAW_y_1"  , y_1         );
    Firebase.setIntAsync(fbdo, "/test/RAW_z_1"  , z_1         );
    Firebase.setIntAsync(fbdo, "/test/RAW_x2"   , x2          );
    Firebase.setIntAsync(fbdo, "/test/RAW_y2"   , y2          );
    Firebase.setIntAsync(fbdo, "/test/RAW_z2"   , z2          );
  }
}
//----------------------------------------------------------------
void read_Flex_Sensor() {
  analogValue = analogRead(flexPin);
  Serial.printf("ADC analog value = %d\n", analogValue);
}
//----------------------------------------------------------------
void read_MPU6050_Sensors() {
  // Read accelerometer and gyroscope data from MPU1
  int16_t ax1, ay1, az1, gx1, gy1, gz1;
  mpu1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1);

  // Read accelerometer and gyroscope data from MPU2
  int16_t ax2, ay2, az2, gx2, gy2, gz2;
  mpu2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

  // Print the data
  Serial.print("MPU1: ");
  //  Serial.print("Accel(XYZ): ");
  //  Serial.print(ax1); Serial.print(", ");
  //  Serial.print(ay1); Serial.print(", ");
  //  Serial.print(az1); Serial.print(" | ");
  //Serial.print("Gyro(XYZ): ");
  x_1 = gx1 + 171;
  y_1 = gy1 - 179;
  z_1 = gz1 + 132;
  //  Serial.print(gx1); Serial.print(", ");
  //  Serial.print(gy1); Serial.print(", ");
  //  Serial.println(gz1);
  if (x_1 > -100 && x_1 < 100) {
    x_1 = 0;
  }
  if (y_1 > -100 && y_1 < 100) {
    y_1 = 0;
  }
  if (z_1 > -100 && z_1 < 100) {
    z_1 = 0;
  }
  Serial.print(x_1); Serial.print(", ");
  Serial.print(y_1); Serial.print(", ");
  Serial.println(z_1);

  Serial.print("MPU2: ");
  //  Serial.print("Accel(XYZ): ");
  //  Serial.print(ax2); Serial.print(", ");
  //  Serial.print(ay2); Serial.print(", ");
  //  Serial.print(az2); Serial.print(" | ");
  x2 = gx2 + 390;
  y2 = gy2 + 80;
  z2 = gz2 + 60;
  //  Serial.print(gx2); Serial.print(", ");
  //  Serial.print(gy2); Serial.print(", ");
  //  Serial.println(gz2);
  if (x2 > -100 && x2 < 100) {
    x2 = 0;
  }
  if (y2 > -100 && y2 < 100) {
    y2 = 0;
  }
  if (z2 > -100 && z2 < 100) {
    z2 = 0;
  }
  Serial.print(x2); Serial.print(", ");
  Serial.print(y2); Serial.print(", ");
  Serial.println(z2);
  Serial.println("------------------------------------------------------");
  delay(500); // Adjust the delay according to your requirements

}

//----------------------------------------------------------------
void setup_Sensors() {

  Wire.begin();
  //set the resolution to 12 bits (0-4096)
  analogReadResolution(12);
  // Initialize MPU6050 sensors with their respective addresses
  mpu1.initialize();
  mpu2.initialize();

}
//----------------------------------------------------------------
void connect_To_WiFi_and_Firebase() {

  WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
  Serial.print("Connecting to Wi-Fi");
  while (WiFi.status() != WL_CONNECTED)
  {
    Serial.print(".");
    delay(300);
  }
  Serial.println();
  Serial.print("Connected with IP: ");
  Serial.println(WiFi.localIP());
  Serial.println();

  Serial.printf("Firebase Client v%s\n\n", FIREBASE_CLIENT_VERSION);

  /* Assign the api key (required) */
  config.api_key = API_KEY;

  /* Assign the user sign in credentials */
  auth.user.email = USER_EMAIL;
  auth.user.password = USER_PASSWORD;

  /* Assign the RTDB URL */
  config.database_url = DATABASE_URL;

  Firebase.reconnectWiFi(true);
  fbdo.setResponseSize(4096);

  String base_path = "/UsersData/";

  /* Assign the callback function for the long running token generation task */
  config.token_status_callback = tokenStatusCallback; // see addons/TokenHelper.h

  /** Assign the maximum retry of token generation */
  config.max_token_generation_retry = 5;

  /* Initialize the library with the Firebase authen and config */
  Firebase.begin(&config, &auth);

  /** Now modify the database rules (if not yet modified)

     The user, <user uid> in this case will be granted to read and write
     at the certain location i.e. "/UsersData/<user uid>".

     If you database rules has been modified, please comment this code out.

     The character $ is to make a wildcard variable (can be any name) represents any node key
     which located at some level in the rule structure and use as reference variable
     in .read, .write and .validate rules

     For this case $userId represents any <user uid> node that places under UsersData node i.e.
     /UsersData/<user uid> which <user uid> is user UID.

     Please check your the database rules to see the changes after run the below code.
  */
  String var = "$userId";
  String val = "($userId === auth.uid && auth.token.premium_account === true && auth.token.admin === true)";
  Firebase.setReadWriteRules(fbdo, base_path, var, val, val, DATABASE_SECRET);

  /** path for user data is now "/UsersData/<user uid>"
     The user UID can be taken from auth.token.uid
  */
}
//----------------------------------------------------------------

//---------------------------------------------------------------------------

/*
  void setup() {
  // Set MPU6050 scale and sensitivity settings if needed
  // mpu1.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  //  mpu1.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  // mpu2.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  // mpu2.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  }
*/