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

LiquidCrystal_I2C lcd(0x27, 16, 2); 
MPU6050 mpu;

int speedPin = A0; 
int pressurePin = A1; 
float speedValue, pressureValue;

const int chipSelect = 10; 

void setup() {
  // Initialize LCD
  lcd.begin(16, 2);
  lcd.print("Telemetry Start");

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();

  // Start Serial Monitor
  Serial.begin(9600);

  // Initialize SD card
  if (!SD.begin(chipSelect)) {
    Serial.println("SD card initialization failed!");
    lcd.clear();
    lcd.print("SD Init Failed");
    return;
  }
  Serial.println("SD card initialized.");
}

void loop() {
  // Read speed value from potentiometer (0-1023) and map to (0-300 km/h)
  speedValue = analogRead(speedPin);
  speedValue = map(speedValue, 0, 1023, 0, 300);

  // Read pressure value (0-1023) and map to PSI (0-100 PSI)
  pressureValue = analogRead(pressurePin);
  pressureValue = map(pressureValue, 0, 1023, 0, 100);

  // Display speed and pressure on the LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Speed: ");
  lcd.print(speedValue);
  lcd.print(" km/h");

  lcd.setCursor(0, 1);
  lcd.print("Pressure: ");
  lcd.print(pressureValue);
  lcd.print(" PSI");

  // Get MPU6050 values (acceleration and orientation)
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Log data to SD card
  logData(speedValue, pressureValue, ax, ay, az, gx, gy, gz);

  // Output MPU data to Serial Monitor
  Serial.print("Accel X: "); Serial.print(ax);
  Serial.print(" | Accel Y: "); Serial.print(ay);
  Serial.print(" | Accel Z: "); Serial.println(az);

  delay(1000); // Adjust delay as needed
}

// Function to log data to SD card
void logData(float speed, float pressure, int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) {
  File dataFile = SD.open("log.txt", FILE_WRITE);

  if (dataFile) {
    dataFile.print("Speed: ");
    dataFile.print(speed);
    dataFile.print(" km/h, Pressure: ");
    dataFile.print(pressure);
    dataFile.print(" PSI, Accel X: ");
    dataFile.print(ax);
    dataFile.print(" | Y: ");
    dataFile.print(ay);
    dataFile.print(" | Z: ");
    dataFile.print(az);
    dataFile.print(", Gyro X: ");
    dataFile.print(gx);
    dataFile.print(" | Y: ");
    dataFile.print(gy);
    dataFile.print(" | Z: ");
    dataFile.println(gz);

    dataFile.close();
    Serial.println("Data logged to SD card.");
  } else {
    Serial.println("Error opening log.txt");
  }
}