#include <Wire.h>
#include <SoftwareSerial.h>
#include <TinyGPS+.h>

// HC-SR04 Pins
#define TRIG_PIN 5
#define ECHO_PIN 18

// MPU6050 I2C Address
#define MPU_ADDR 0x68

// GPS Module (NEO-6M)
TinyGPSPlus gps;
HardwareSerial gpsSerial(1); // UART1 for GPS

// SIM800L Module
HardwareSerial sim800(2); // UART2 for GSM

// Variables for MPU6050
int16_t accX, accY, accZ, gyroX, gyroY, gyroZ;

void setup() {
  Serial.begin(115200);

  // HC-SR04 Setup
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // MPU6050 Setup
  Wire.begin();
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0);    // Wake up MPU6050
  Wire.endTransmission(true);

  // GPS Serial Setup
  gpsSerial.begin(9600, SERIAL_8N1, 16, 17); // RX, TX

  // SIM800L Serial Setup
  sim800.begin(9600, SERIAL_8N1, 26, 27); // RX, TX

  Serial.println("System Initialized.");
}

void loop() {
  // 1️⃣ Read Distance from HC-SR04
  long duration;
  float distance;
  
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  duration = pulseIn(ECHO_PIN, HIGH);
  distance = duration * 0.034 / 2;
  Serial.print("Distance (cm): ");
  Serial.println(distance);

  // 2️⃣ Read Accelerometer & Gyro from MPU6050
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B); // Starting register for Accel data
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_ADDR, 14, true);

  accX = Wire.read() << 8 | Wire.read();
  accY = Wire.read() << 8 | Wire.read();
  accZ = Wire.read() << 8 | Wire.read();
  gyroX = Wire.read() << 8 | Wire.read();
  gyroY = Wire.read() << 8 | Wire.read();
  gyroZ = Wire.read() << 8 | Wire.read();

  Serial.print("AccX: "); Serial.print(accX);
  Serial.print(" | AccY: "); Serial.print(accY);
  Serial.print(" | AccZ: "); Serial.println(accZ);

  // 3️⃣ Read GPS Data from NEO-6M
  while (gpsSerial.available() > 0) {
    gps.encode(gpsSerial.read());
    if (gps.location.isUpdated()) {
      Serial.print("Lat: ");
      Serial.print(gps.location.lat(), 6);
      Serial.print(" | Lon: ");
      Serial.println(gps.location.lng(), 6);
    }
  }

  // 4️⃣ Send Data via SIM800L
  sim800.println("AT");
  delay(500);
  sim800.println("AT+CMGF=1");
  delay(500);
  sim800.println("AT+CMGS=\"+1234567890\""); // Replace with real number
  delay(500);
  sim800.print("Distance: ");
  sim800.print(distance);
  sim800.print("cm | Lat: ");
  sim800.print(gps.location.lat(), 6);
  sim800.print(" | Lon: ");
  sim800.print(gps.location.lng(), 6);
  sim800.write(26); // Ctrl+Z to send
  delay(3000);

  delay(1000); // Main loop delay
}