#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; // Pin for the vibration motor

const int threshold = 50; // Threshold for detecting movement
const int resetThreshold = 100; // Threshold for detecting joystick button press
const long morseDelay = 500; // Delay for Morse code playback
const long buzzerDelay = 30000; // Delay between buzzer plays
int ThresholdError = 750; // Additive value for thresholds

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

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

// Keypad pins
const int rowPins[4] = {9, 10, 11, 12}; // Rows 0 to 3
const int colPins[4] = {13, 14, 15, 16}; // Columns 0 to 3

// Ensure proper pin connections for your LCD
LiquidCrystal_I2C lcd(0x27, 16, 2); // Set the LCD I2C address to 0x27, and the LCD size to 16x2

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 ledEnabled = true; // Flag to track LED functionality status
bool lcdEnabled = true; // Flag to track LCD functionality status
bool buzzerEnabled = true; // Flag to track buzzer functionality status

char keypadInput[17]; // Array to store keypad input
int keypadIndex = 0; // Index to keep track of the current position in keypadInput array

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();

  // Initialize the LCD
  lcd.init();
  lcd.backlight(); // Turn on the backlight
  lcd.print("Setup Complete");

  // Initialize keypad
  for (int i = 0; i < 4; i++) {
    pinMode(rowPins[i], INPUT_PULLUP);
    pinMode(colPins[i], OUTPUT);
  }

  memset(keypadInput, 0, sizeof(keypadInput)); // Clear keypadInput array
}

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 (ledEnabled && currentX < xThresholdMin - threshold) {
    Left(currentTime - lastButtonPressTime >= buzzerDelay);
  } else {
    digitalWrite(ledPins[0], LOW); // Turn off LED
  }
  if (ledEnabled && currentX > xThresholdMax + threshold) {
    Right(currentTime - lastButtonPressTime >= buzzerDelay);
  } else {
    digitalWrite(ledPins[1], LOW); // Turn off LED
  }

  if (ledEnabled && currentY < yThresholdMin - threshold) {
    Forward(currentTime - lastButtonPressTime >= buzzerDelay);
  } else {
    digitalWrite(ledPins[2], LOW); // Turn off LED
  }

  if (ledEnabled && currentY > yThresholdMax + threshold) {
    Backward(currentTime - lastButtonPressTime >= buzzerDelay);
  } else {
    digitalWrite(ledPins[3], LOW); // Turn off LED
  }

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

  // Check for keypad input
  readKeypad();
}

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

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

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

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

void indicateDirection(int index, const char* message, void (*morseFunc)(), bool morseActive) {
  if (morseActive) morseFunc(); // Play Morse code
  if (lcdEnabled) lcd.print(message); // Display direction on LCD
}

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 readKeypad() {
  for (int col = 0; col < 4; col++) {
    pinMode(colPins[col], INPUT);
    for (int row = 0; row < 4; row++) {
      if (digitalRead(rowPins[row]) == LOW) {
        char key = getKey(row, col);
        if (key != '\0') { // Check if key is not null character
          if (key == 'A') {
            enableAll();
          } else if (key == 'B') {
            disableLEDAndLCD();
          } else if (key == 'C') {
            disableBuzzer();
          }
          delay(500); // Debounce delay
        }
      }
    }
    pinMode(colPins[col], OUTPUT);
    digitalWrite(colPins[col], HIGH);
  }
}

char getKey(int row, int col) {
  const char keys[4][4] = {
    {'1', '2', '3', 'A'},
    {'4', '5', '6', 'B'},
    {'7', '8', '9', 'C'},
    {'*', '0', '#', 'D'}
  };
  return keys[row][col];
}

void enableAll() {
  ledEnabled = true;
  lcdEnabled = true;
  buzzerEnabled = true;
  for (int i = 0; i < 4; i++) {
    digitalWrite(ledPins[i], HIGH); // Turn on all LEDs
  }
  lcd.backlight(); // Turn on LCD backlight
}

void disableLEDAndLCD() {
  ledEnabled = false;
  lcdEnabled = false;
  for (int i = 0; i < 4; i++) {
    digitalWrite(ledPins[i], LOW); // Turn off all LEDs
  }
  lcd.noBacklight(); // Turn off LCD backlight
}

void disableBuzzer() {
  buzzerEnabled = false;
}