// ------------------- Libraries -------------------
#include <WiFi.h>                 // Wi-Fi connectivity
#include <PubSubClient.h>         // MQTT protocol
#include <BluetoothSerial.h>      // Bluetooth communication
#include <Wire.h>                 // I2C bus
#include <Adafruit_GFX.h>         // OLED graphics
#include <Adafruit_SSD1306.h>     // OLED display

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

// ------------------- Motor Driver Pins -------------------
#define IN1 26
#define IN2 27
#define IN3 32
#define IN4 33

// ------------------- Joystick Pins -------------------
#define VRX 34  // X-axis
#define VRY 35  // Y-axis

// ------------------- Ultrasonic Pins -------------------
#define TRIG 5
#define ECHO 18

// ------------------- Wi-Fi + MQTT -------------------
const char* ssid = "YOUR_WIFI";       // 🔑 Wi-Fi SSID
const char* password = "YOUR_PASS";   // 🔑 Wi-Fi Password
const char* mqtt_server = "broker.hivemq.com"; // Free MQTT broker
WiFiClient espClient;
PubSubClient client(espClient);

// ------------------- Bluetooth -------------------
BluetoothSerial SerialBT;

// ------------------- Functions -------------------

// 🚧 Read distance using ultrasonic sensor
long readDistance() {
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long duration = pulseIn(ECHO, HIGH);
  return duration * 0.034 / 2; // Convert time → cm
}

// 🛞 Motor control functions
void moveForward() {
  digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW);
  digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW);
}
void moveBackward() {
  digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH);
  digitalWrite(IN3,LOW); digitalWrite(IN4,HIGH);
}
void turnLeft() {
  digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH);
  digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW);
}
void turnRight() {
  digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW); digitalWrite(IN4,HIGH);
}
void stopMotors() {
  digitalWrite(IN1,LOW); digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW); digitalWrite(IN4,LOW);
}

// 🌐 MQTT callback (when command received)
void callback(char* topic, byte* payload, unsigned int length) {
  String msg;
  for (int i=0;i<length;i++) msg += (char)payload[i];
  Serial.println("MQTT Command: " + msg);

  if (msg=="F") moveForward();
  else if (msg=="B") moveBackward();
  else if (msg=="L") turnLeft();
  else if (msg=="R") turnRight();
  else stopMotors();
}

// ------------------- Setup -------------------
void setup() {
  Serial.begin(115200);

  // OLED Init
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setCursor(0,0); display.println("Smart Wheelchair Init...");
  display.display();

  // Motor Pins
  pinMode(IN1,OUTPUT); pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT); pinMode(IN4,OUTPUT);

  // Ultrasonic Pins
  pinMode(TRIG,OUTPUT); pinMode(ECHO,INPUT);

  // Wi-Fi Connect
  WiFi.begin(ssid,password);
  while(WiFi.status()!=WL_CONNECTED){ delay(500); Serial.print("."); }
  Serial.println("WiFi connected");

  // MQTT setup
  client.setServer(mqtt_server,1883);
  client.setCallback(callback);

  // Bluetooth Init
  SerialBT.begin("ESP32_Wheelchair");
  Serial.println("Bluetooth Ready");
}

// ------------------- Main Loop -------------------
void loop() {
  // --- Safety: Ultrasonic ---
  long dist = readDistance();
  if (dist < 20) {
    stopMotors();
    display.clearDisplay();
    display.setCursor(0,0);
    display.println("⚠ Obstacle Ahead!");
    display.display();
    return; // Stop movement if obstacle too close
  }

  // --- Joystick Input ---
  int x = analogRead(VRX);
  int y = analogRead(VRY);

  if (x > 3000) turnRight();
  else if (x < 1000) turnLeft();
  else if (y > 3000) moveForward();
  else if (y < 1000) moveBackward();
  else stopMotors();

  // --- Wi-Fi MQTT Loop ---
  if (!client.connected()) {
    if (client.connect("WheelchairClient")) {
      client.subscribe("wheelchair/control");
    }
  }
  client.loop();

  // --- Bluetooth Input ---
  if (SerialBT.available()) {
    char c = SerialBT.read();
    if (c=='F') moveForward();
    else if (c=='B') moveBackward();
    else if (c=='L') turnLeft();
    else if (c=='R') turnRight();
    else stopMotors();
  }

  // --- OLED Display Update ---
  display.clearDisplay();
  display.setCursor(0,0);
  display.print("Dist: "); display.println(dist);
  display.print("WiFi: "); display.println(WiFi.localIP());
  display.println("BT+MQTT Ready");
  display.display();

  delay(200);
}