#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>

MPU6050 mpu;

const int buzzerPin = 3; // Digital pin for active buzzer
const int ledPins[] = {4, 5, 6, 7}; // Digital pins for LEDs
const int vibrationMotorPin = 8; // Example pin for the vibration motor

const int threshold = 50; // Threshold for detecting movement
const long morseDelay = 500; // Delay for Morse code playback

long currentX = 0; // Current X coordinate
long currentY = 0; // Current Y coordinate

float initialXAccel = 0.0; // Initial acceleration in X direction
float initialYAccel = 0.0; // Initial acceleration in Y direction

float velocityX = 0.0; // Velocity in X direction
float velocityY = 0.0; // Velocity in Y direction

float displacementX = 0.0; // Displacement in X direction
float displacementY = 0.0; // Displacement in Y direction

unsigned long lastReadTime = 0; // Last time sensor data was read

void setup() {
  Serial.begin(9600); // Initialize serial communication
  for (int i = 0; i < 4; i++) {
    pinMode(ledPins[i], OUTPUT); // Set LED pins as output
  }
  pinMode(buzzerPin, OUTPUT); // Set buzzer pin as output
  pinMode(vibrationMotorPin, OUTPUT); // Set vibration motor pin as output

  // Initialize the Wire library (necessary for I2C communication)
  Wire.begin();

  // Initialize the MPU6050
  mpu.initialize();

  // Calibrate the accelerometer
  calibrateAccelerometer();

  // Set the initial time
  lastReadTime = millis();
}

void loop() {
  unsigned long currentTime = millis();
  float deltaTime = (currentTime - lastReadTime) / 1000.0; // Time difference in seconds

  // Read sensor data
  readSensorData();

  // Integrate acceleration to get velocity
  velocityX += (currentX * deltaTime);
  velocityY += (currentY * deltaTime);

  // Integrate velocity to get displacement
  displacementX += (velocityX * deltaTime);
  displacementY += (velocityY * deltaTime);

  // Update the last read time
  lastReadTime = currentTime;

  // Check for direction functions
  if (currentX < -threshold) {
    Left();
  } else {
    digitalWrite(ledPins[0], LOW); // Turn off LED
  }
  if (currentX > threshold) {
    Right();
  } else {
    digitalWrite(ledPins[1], LOW); // Turn off LED
  }

  if (currentY < -threshold) {
    Forward();
  } else {
    digitalWrite(ledPins[2], LOW); // Turn off LED
  }

  if (currentY > threshold) {
    Backward();
  } else {
    digitalWrite(ledPins[3], LOW); // Turn off LED
  }
}

void readSensorData() {
  // Read accelerometer data from MPU6050
  int16_t accelX, accelY, accelZ;
  mpu.getAcceleration(&accelX, &accelY, &accelZ);

  // Calculate current accelerations
  currentX = accelX - initialXAccel;
  currentY = accelY - initialYAccel;

  // Output current accelerations
  Serial.print("Current X acceleration: ");
  Serial.print(currentX);
  Serial.print(", Current Y acceleration: ");
  Serial.println(currentY);
}

void calibrateAccelerometer() {
  // Perform accelerometer calibration by averaging readings over a short period
  const int numSamples = 100;
  long sumX = 0, sumY = 0;

  for (int i = 0; i < numSamples; i++) {
    int16_t accelX, accelY, accelZ;
    mpu.getAcceleration(&accelX, &accelY, &accelZ);
    sumX += accelX;
    sumY += accelY;
    delay(10); // Delay between samples
  }

  // Calculate the average acceleration values
  initialXAccel = sumX / (float)numSamples;
  initialYAccel = sumY / (float)numSamples;

  // Output calibration results
  Serial.print("Calibrated X acceleration: ");
  Serial.print(initialXAccel);
  Serial.print(", Calibrated Y acceleration: ");
  Serial.println(initialYAccel);
}

void Left() {
  // Implement left direction logic here
}

void Right() {
  // Implement right direction logic here
}

void Forward() {
  // Implement forward direction logic here
}

void Backward() {
  // Implement backward direction logic here
}