#include <Wire.h>
#include <MPU6050.h>
#include <Keypad.h> // Include the Keypad library

MPU6050 mpu;

const int buzzerPin = 3; // Digital pin for active buzzer
const int vibrationMotorPin = 2; // Pin for the vibration motor

const int threshold = 50; // Threshold for detecting movement
const long morseDelay = 500; // Delay for Morse code playback
const long buzzerDelay = 30000; // Delay between buzzer plays
int ThresholdError = 2000; // Additive value for thresholds

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

const int xThresholdMin = 10000; // Minimum threshold for X coordinate
const int xThresholdMax = xThresholdMin + ThresholdError; // Maximum threshold for X coordinate
const int yThresholdMin = 15000; // Minimum threshold for Y coordinate
const int yThresholdMax = yThresholdMin + ThresholdError; // Maximum threshold for 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

const byte ROWS = 4; // Number of rows in the keypad
const byte COLS = 4; // Number of columns in the keypad

// Define the keymap
char keys[ROWS][COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

// Define the row and column pins connected to the keypad
byte rowPins[ROWS] = {13, 12, 11, 10}; // Rows 0 to 3
byte colPins[COLS] = {7, 7, 7, 6}; // Columns 0 to 3

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS); // Create a Keypad object

unsigned long lastReadTime = 0; // Last time sensor data was read
unsigned long lastButtonPressTime = 0; // Last time button was pressed
unsigned long lastBuzzerTime = 0; // Last time buzzer was played

bool buzzerPlaying = false; // Flag to track buzzer playing status
bool buzzerEnabled = true; // Flag to track buzzer functionality status

const int redPinX = 8; // Red pin of RGB LED for X threshold
const int greenPinX = 9; // Green pin of RGB LED for X threshold
const int bluePinX = 10; // Blue pin of RGB LED for X threshold

const int redPinY = 5; // Red pin of RGB LED for Y threshold
const int greenPinY = 4; // Green pin of RGB LED for Y threshold
const int bluePinY = 3; // Blue pin of RGB LED for Y threshold

void setup() {
  pinMode(LED_BUILTIN, OUTPUT); // Set the onboard LED pin as output

  // Blink the onboard LED three times
  for (int i = 0; i < 3; i++) {
    digitalWrite(LED_BUILTIN, HIGH); // Turn on the LED
    delay(500); // Wait for 500 milliseconds
    digitalWrite(LED_BUILTIN, LOW); // Turn off the LED
    delay(500); // Wait for 500 milliseconds
  }

  Serial.begin(9600); // Initialize serial communication
  pinMode(buzzerPin, OUTPUT); // Set buzzer pin as output
  pinMode(vibrationMotorPin, OUTPUT); // Set vibration motor pin as output

  pinMode(redPinX, OUTPUT); // Set red pin of RGB LED for X threshold as output
  pinMode(greenPinX, OUTPUT); // Set green pin of RGB LED for X threshold as output
  pinMode(bluePinX, OUTPUT); // Set blue pin of RGB LED for X threshold as output

  pinMode(redPinY, OUTPUT); // Set red pin of RGB LED for Y threshold as output
  pinMode(greenPinY, OUTPUT); // Set green pin of RGB LED for Y threshold as output
  pinMode(bluePinY, OUTPUT); // Set blue pin of RGB LED for Y threshold 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() {
  // Wait for a button press
  char keyPressed = keypad.getKey();

  // Update serial monitor for debugging
  if (keyPressed != NO_KEY) {
    Serial.print("Button pressed: ");
    Serial.println(keyPressed);

    // Start accelerometer tracking only after a button press
    while (true) {
      // Your code for sensor data reading and other operations
      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 < xThresholdMin - threshold) {
        Left(currentTime - lastButtonPressTime >= buzzerDelay);
      }
      if (currentX > xThresholdMax + threshold) {
        Right(currentTime - lastButtonPressTime >= buzzerDelay);
      }

      if (currentY < yThresholdMin - threshold) {
        Forward(currentTime - lastButtonPressTime >= buzzerDelay);
      }

      if (currentY > yThresholdMax + threshold) {
        Backward(currentTime - lastButtonPressTime >= buzzerDelay);
      }

      if (currentTime - lastButtonPressTime >= buzzerDelay) {
        lastButtonPressTime = currentTime; // Reset timer
      }

      // Update RGB LED based on proximity to thresholds
      updateRGBLED();
        // Debugging: Print LED values
  Serial.print("RGB LED Values - X: ");
  Serial.print(analogRead(redPinX));
  Serial.print(", ");
  Serial.print(analogRead(greenPinX));
  Serial.print(", ");
  Serial.println(analogRead(bluePinX));

  Serial.print("RGB LED Values - Y: ");
  Serial.print(analogRead(redPinY));
  Serial.print(", ");
  Serial.print(analogRead(greenPinY));
  Serial.print(", ");
  Serial.println(analogRead(bluePinY));

  // Add a short delay to avoid flooding the serial monitor
  delay(100);
    }
  }
}

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(bool morseActive) {
  indicateDirection("Left", morseL, morseActive);
}

void Right(bool morseActive) {
  indicateDirection("Right", morseR, morseActive);
}

void Forward(bool morseActive) {
  indicateDirection("Forward", morseF, morseActive);
}

void Backward(bool morseActive) {
  indicateDirection("Backward", morseB, morseActive);
}

void indicateDirection(const char* message, void (*morseFunc)(), bool morseActive) {
  if (morseActive) morseFunc(); // Play Morse code
}

void morseL() {
  playMorse(".-..");
}

void morseR() {
  playMorse(".-.");
}

void morseF() {
  playMorse("..-.");
}

void morseB() {
  playMorse("-...");
}

void playMorse(const char* morseCode) {
  // Loop through Morse code characters and play
  for (const char* p = morseCode; *p; p++) {
    // New code to control vibration motor
    if (*p == '.') {
      dot(); // Vibrate for dot
    } else if (*p == '-') {
      dash(); // Vibrate for dash
    } else if (*p == ' ') {
      // Pause between characters
      delay(2 * morseDelay);
    }
  }
}

// Function to vibrate for a dot
void dot() {
  digitalWrite(vibrationMotorPin, HIGH); // Vibrate for dot
  delay(morseDelay);
  digitalWrite(vibrationMotorPin, LOW); // Turn off vibration motor
  delay(morseDelay); // Pause between dots
}

// Function to vibrate for a dash
void dash() {
  digitalWrite(vibrationMotorPin, HIGH); // Vibrate for dash
  delay(3 * morseDelay);
  digitalWrite(vibrationMotorPin, LOW); // Turn off vibration motor
  delay(morseDelay); // Pause between dashes
}

void playBuzzer() {
  if (buzzerEnabled) {
    buzzerPlaying = true;
    Serial.println("Playing buzzer");

    // Play your desired buzzer tone here
    digitalWrite(buzzerPin, HIGH);
    delay(500); // Adjust as needed
    digitalWrite(buzzerPin, LOW);

    lastBuzzerTime = millis();
    buzzerPlaying = false;
  }
}

/*
void updateRGBLED() {
  // Update RGB LED for X threshold
  int xGradient = map(currentX, xThresholdMin, xThresholdMax, 0, 255);
  analogWrite(redPinX, 255 - xGradient); // Red decreases as the user approaches the threshold
  analogWrite(greenPinX, xGradient); // Green increases as the user approaches the threshold
  analogWrite(bluePinX, 0); // Blue remains constant (e.g., set to 0)

  // Update RGB LED for Y threshold
  int yGradient = map(currentY, yThresholdMin, yThresholdMax, 0, 255);
  analogWrite(redPinY, 255 - yGradient); // Red decreases as the user approaches the threshold
  analogWrite(greenPinY, 0); // Green remains constant (e.g., set to 0)
  analogWrite(bluePinY, yGradient); // Blue increases as the user approaches the threshold
}


void updateRGBLED() {
  // Update RGB LED for X threshold
  int xGradient = map(abs(currentX), 0, ThresholdError, 0, 255); // Calculate gradient for X
  analogWrite(redPinX, xGradient); // Red increases as the user moves away from the threshold
  analogWrite(greenPinX, 0); // Green remains constant (e.g., set to 0)
  */

  void updateRGBLED() {
  // Update RGB LED for X threshold
  int xGradient = map(abs(currentX), 0, ThresholdError, 255, 0); // Calculate gradient for X
  analogWrite(redPinX, xGradient); // Red decreases as the user approaches the threshold
  analogWrite(greenPinX, 255 - xGradient); // Green increases as the user approaches the threshold
  analogWrite(bluePinX, 0); // Blue remains constant (e.g., set to 0)

  // Update RGB LED for Y threshold
  int yGradient = map(abs(currentY), 0, ThresholdError, 255, 0); // Calculate gradient for Y
  analogWrite(redPinY, 255 - yGradient); // Red increases as the user approaches the threshold
  analogWrite(greenPinY, 0); // Green remains constant (e.g., set to 0)
  analogWrite(bluePinY, yGradient); // Blue decreases as the user approaches the threshold
}