#include <Wire.h> //Assigned libraries
#include <MPU6050.h>

MPU6050 sensor;

// RAW values (not-processed) from the accelerometer on x,y,z axis
int ax, ay, az; //Declares integer variables for storing raw accelerometer data along the x, y, and z axes.
float array1[]= {0, 0, 0}; //Initializes a float array to store calculated angles.
const int vibrator_module= 17 ; // (GPIO 5) Defines a constant for the GPIO pin connected to the vibrator module.
int intensity = 150; // must have a range of value between 0 and 255 for the PWM to kick in

void setup() { 
  pinMode(vibrator_module, OUTPUT); //Sets the vibrator module pin as an output.

  Wire.begin(); //Initializes the I2C communication.
  Serial.begin(115200); //Starts the serial communication at a baud rate of 115200.
  sensor.initialize(); //Initializes the MPU6050 sensor.

  // Verifying the connection
  if (sensor.testConnection()) { Serial.println("MPU6050 connection successful"); } //Checks if the sensor is connected; if yes, prints a success message.
  else { Serial.println("MPU6050 connection failed"); //If the connection fails, it prints a failure message.
    while (1); //Enters an infinite loop to halt further execution if the connection fails.
  }
}

void loop() {
  // Extract data from accelerometer & gyroscope
  int16_t ax, ay, az, gx, gy, gz; //Declares 16-bit integer variables for raw accelerometer and gyroscope data.
  //int16_t gx, gy, gz;

  sensor.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //Retrieves motion data (accelerometer and gyroscope readings) from the sensor.


  // Calculating orientation
  float accel_ang_x=atan(ax/sqrt(pow(ay,2) + pow(az,2)))*(180.0/3.14); //Calculates the angle of inclination around the x-axis using arctangent and stores it in accel_ang_x.
  float accel_ang_y=atan(ay/sqrt(pow(ax,2) + pow(az,2)))*(180.0/3.14); //Calculates the angle of inclination around the y-axis using arctangent and stores it in accel_ang_y.

  assign_values(accel_ang_x, accel_ang_y, array1); //Calls a function to assign the calculated angles to the array.
  vibration(accel_ang_y, vibrator_module); //Calls a function to activate the vibrator based on the calculated y-axis angle.
  delay(500); //Pauses the loop for 500 milliseconds before repeating.
}

void assign_values(int x, int y, float array[]){ //Defines a function assign_values that takes two integers and a float array as parameters.
    array[0] = x; 
    array[1] = y;   //Assigns the value of x and y to the second element of the array.
    Serial.print("Angle in X: "); //Prints the message "Angle in X: " to the serial monitor.
    Serial.print(array[0]);  //Prints the value of the first element of the array (angle in X) to the serial monitor.
    Serial.print("  Angle in Y: "); //Prints the message "Angle in Y: " to the serial monitor.
    Serial.println(array[1]); //Prints the value of the second element of the array (angle in Y) and moves to a new line in the serial monitor.
}

void vibracion(int angle, int vibrator){  //Defines a function vibracion that takes an integer angle and a vibrator pin as parameters.
  if (angle > 45){   
      int vibration = 100; 
      analogWrite(vibrator, vibration); // 75% PWM Signal on PIN 7 (Writes a PWM signal with the value of vibration (100) to the specified vibrator pin.)
      //Serial.println(vibration); 
      //delay(300);
  }

  else if(angle < 45) { 
      int vibration = 250;
      analogWrite(vibrator, vibration); // 75% PWM Signal on PIN 7 (Writes a PWM signal with the value of vibration (250) to the specified vibrator pin.)
      delay(200); 
      analogWrite(vibrator, 100); //Writes a lower PWM signal (100) to the vibrator pin.
      delay(200);
  }
}