#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TinyGPSPlus.h>
#include <WiFi.h>

// --- PIN DEFINITIONS (From Wiring Table) ---
#define VIB_MOTOR_PIN  25
#define BUZZER_PIN     26
#define SOS_BUTTON_PIN 27
#define RED_LED_PIN    32
#define GREEN_LED_PIN  33

// UART Pins
#define GPS_RX_PIN 16 // ESP32 RX2 <- GPS TX
#define GPS_TX_PIN 17 // ESP32 TX2 -> GPS RX
#define SIM_RX_PIN 19 // ESP32 RX1 <- SIM TXD
#define SIM_TX_PIN 18 // ESP32 TX1 -> SIM RXD

// OLED Display Config
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Sensors & Modules
Adafruit_MPU6050 mpu;
TinyGPSPlus gps;
HardwareSerial SerialGPS(2); // Use ESP32 UART2
HardwareSerial SerialSIM(1); // Use ESP32 UART1

// --- CONFIGURATION ---
const String CAREGIVER_PHONE = "+1234567890"; // Replace with real phone number
const char* WIFI_SSID = "YOUR_WIFI_SSID";
const char* WIFI_PASS = "YOUR_WIFI_PASS";

unsigned long lastFallTime = 0;

// Function prototypes
void triggerAlarm(String reason);
void sendSMS(String alertText, String lat, String lng);

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

  // 1. Initialize GPIO Pins
  pinMode(VIB_MOTOR_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  pinMode(SOS_BUTTON_PIN, INPUT_PULLUP); // SOS button connected to GND

  digitalWrite(GREEN_LED_PIN, HIGH); // System Power/Active Indicator
  digitalWrite(RED_LED_PIN, LOW);

  // 2. Initialize Hardware Serials
  SerialGPS.begin(9600, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
  SerialSIM.begin(9600, SERIAL_8N1, SIM_RX_PIN, SIM_TX_PIN);

  // 3. Initialize Shared I2C (Pins 21 & 22 default on ESP32)
  Wire.begin(21, 22);

  // 4. Initialize OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
  } else {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 10);
    display.println("FallGuard Ready...");
    display.display();
  }

  // 5. Initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
  } else {
    mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
    mpu.setGyroRange(MPU6050_RANGE_500_DEG);
    mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  }

  // 6. Connect WiFi (Backend ready state)
  WiFi.begin(WIFI_SSID, WIFI_PASS);

  // 7. Initialize SIM800L for SMS
  delay(3000); // Give SIM module time to register to network
  SerialSIM.println("AT");
  delay(500);
  SerialSIM.println("AT+CMGF=1"); // Set SMS to Text Mode
  delay(500);
}

void loop() {
  // Feed data to TinyGPS++ parser continually
  while (SerialGPS.available() > 0) {
    gps.encode(SerialGPS.read());
  }

  // Check SOS Button (Active Low)
  if (digitalRead(SOS_BUTTON_PIN) == LOW) {
    triggerAlarm("SOS Button Pressed!");
    delay(5000); // Debounce to prevent multiple triggers
  }

  // Read MPU6050
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  // Basic Fall Detection Algorithm (Magnitude of Acceleration)
  // Calculate the total 3D acceleration vector
  float accMag = sqrt(pow(a.acceleration.x, 2) + pow(a.acceleration.y, 2) + pow(a.acceleration.z, 2));

  // If sudden impact is detected (Threshold ~25 m/s^2, tune as needed)
  if (accMag > 25.0) {
    if (millis() - lastFallTime > 10000) { // 10 second cooldown between alerts
      lastFallTime = millis();
      triggerAlarm("Possible fall detected.");
    }
  }

  delay(20); // Small loop delay for stability
}

void triggerAlarm(String reason) {
  Serial.println(reason);

  // 1. Activate Local Alarm (Buzzer, Motor, Red LED)
  digitalWrite(RED_LED_PIN, HIGH);
  digitalWrite(VIB_MOTOR_PIN, HIGH);
  digitalWrite(BUZZER_PIN, HIGH);

  // 2. Update OLED Display
  display.clearDisplay();
  display.setCursor(0, 10);
  display.println("ALERT!");
  display.println(reason);
  display.display();

  // 3. Format GPS Data
  String lat = "Unknown";
  String lng = "Unknown";
  if (gps.location.isValid()) {
    lat = String(gps.location.lat(), 6);
    lng = String(gps.location.lng(), 6);
  }

  // 4. Send SMS Alert
  sendSMS(reason, lat, lng);

  // 5. Deactivate Local Alarm after alert completes
  digitalWrite(RED_LED_PIN, LOW);
  digitalWrite(VIB_MOTOR_PIN, LOW);
  digitalWrite(BUZZER_PIN, LOW);
  
  display.clearDisplay();
  display.setCursor(0, 10);
  display.println("FallGuard Ready...");
  display.display();
}

void sendSMS(String alertText, String lat, String lng) {
  // Start SMS command with target phone number
  SerialSIM.println("AT+CMGS=\"" + CAREGIVER_PHONE + "\"");
  delay(1000);

  // Construct message matching the "SMS Format (Example)" in diagram
  String smsBody = alertText + "\n\n";
  smsBody += "Device: armband-001\n";
  
  if (lat != "Unknown") {
    smsBody += "Latitude: " + lat + "\n";
    smsBody += "Longitude: " + lng + "\n";
    smsBody += "Location: https://maps.google.com/?q=" + lat + "," + lng + "\n\n";
  } else {
    smsBody += "Location: GPS signal currently unavailable.\n\n";
  }
  
  smsBody += "Please check immediately.";

  // Send body and CTRL+Z termination character
  SerialSIM.print(smsBody);
  delay(100);
  SerialSIM.write(26); 
  delay(5000); // Wait for SIM800L to process and send
}