#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>


#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
#define WIFI_CHANNEL 6


const char *MQTT_SERVER = "0.tcp.ap.ngrok.io"; 
const uint16_t MQTT_PORT = 10735;

const char *MQTT_CLIENT_NAME = "Arif_Final_Robot"; 
const char *TOPIC_DIST = "arif/distance";
const char *TOPIC_SENSORS = "arif/sensors";
const char *TOPIC_CMD = "arif/command"; 


WiFiClient espClient; 
PubSubClient mqttClient(espClient); 
Adafruit_SSD1306 display(128, 64, &Wire, -1);
Servo radarServo;


const int trigPin = 5;
const int echoPin = 18;
const int leftSensorPin = 25; 
const int rightSensorPin = 26; 

const int stepPin1 = 19; const int dirPin1 = 32; 
const int stepPin2 = 33; const int dirPin2 = 23;

const int servoPin = 15;
const int buzzerPin = 4;


const int obstacleThreshold = 20; 
bool isStopped = false; 
unsigned long lastTelemetry = 0; 
int servoPos = 90;
int servoDir = 2; 


void moveForward() {
  digitalWrite(dirPin1, HIGH); digitalWrite(dirPin2, HIGH);
  digitalWrite(stepPin1, HIGH); digitalWrite(stepPin2, HIGH);
  delayMicroseconds(1000); 
  digitalWrite(stepPin1, LOW); digitalWrite(stepPin2, LOW);
  delayMicroseconds(1000);
}
void turnRight() {
  digitalWrite(dirPin1, HIGH); digitalWrite(dirPin2, LOW);
  digitalWrite(stepPin1, HIGH); digitalWrite(stepPin2, HIGH);
  delayMicroseconds(2000); 
  digitalWrite(stepPin1, LOW); digitalWrite(stepPin2, LOW);
  delayMicroseconds(2000);
}
void turnLeft() {
  digitalWrite(dirPin1, LOW); digitalWrite(dirPin2, HIGH);
  digitalWrite(stepPin1, HIGH); digitalWrite(stepPin2, HIGH);
  delayMicroseconds(2000); 
  digitalWrite(stepPin1, LOW); digitalWrite(stepPin2, LOW);
  delayMicroseconds(2000);
}
int getDistance() {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) * 0.034 / 2;
}


void Callback(char* topic, byte* payload, unsigned int length) {
  String message = "";
  for (int i = 0; i < length; i++) message += (char)payload[i];
  
  // Print Command to Serial
  Serial.print(">>> INCOMING COMMAND: "); 
  Serial.println(message);
  
  // Show Command on OLED
  display.fillRect(0, 50, 128, 14, BLACK); 
  display.setCursor(0, 50);
  display.print("CMD: "); display.print(message);
  display.display();

  if (String(topic) == TOPIC_CMD) {
    if (message == "STOP") isStopped = true;
    else if (message == "GO") isStopped = false;
  }
}

void ConnectMqtt() {
  if (!mqttClient.connected()) {
    if (mqttClient.connect(MQTT_CLIENT_NAME)) {
      Serial.println("--- MQTT CONNECTED TO CLOUD ---");
      
      display.fillRect(0, 0, 128, 10, BLACK);
      display.setCursor(0,0);
      display.println("MQTT: ONLINE");
      display.display();
      
      mqttClient.subscribe(TOPIC_CMD); 
    }
  }
}

void setup() {
  Serial.begin(115200);
  
  // 1. Setup Pins
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  pinMode(leftSensorPin, INPUT); pinMode(rightSensorPin, INPUT);
  pinMode(stepPin1, OUTPUT); pinMode(dirPin1, OUTPUT);
  pinMode(stepPin2, OUTPUT); pinMode(dirPin2, OUTPUT);
  pinMode(buzzerPin, OUTPUT);

  // 2. Setup Servo & Screen
  radarServo.attach(servoPin);
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 Failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0);
  display.println("SYSTEM BOOT...");
  display.display();


  WiFi.begin(WIFI_SSID, WIFI_PASSWORD, WIFI_CHANNEL);
  Serial.print("Connecting WiFi");
  while (WiFi.status() != WL_CONNECTED) { 
    delay(100); 
    Serial.print("."); 
  }
  Serial.println("\nWiFi OK!");
  
  display.println("WiFi Connected!");
  display.display();

  mqttClient.setServer(MQTT_SERVER, MQTT_PORT);
  mqttClient.setCallback(Callback);
}


void loop() {
  ConnectMqtt();
  mqttClient.loop();


  static unsigned long lastServoMove = 0;
  
  if (millis() - lastServoMove > 50) { 
    lastServoMove = millis();
    
    servoPos += (1 * servoDir); 
    if (servoPos >= 135 || servoPos <= 45) servoDir *= -1; 
    radarServo.write(servoPos);
  }

 
  int dist = getDistance();
  int leftVal = digitalRead(leftSensorPin);
  int rightVal = digitalRead(rightSensorPin);

  
  if (millis() - lastTelemetry > 2000) {
    lastTelemetry = millis();
    

    Serial.print("[DATA] Dist: "); Serial.print(dist);
    Serial.print("cm | L: "); Serial.print(leftVal);
    Serial.print(" R: "); Serial.println(rightVal);

    
    mqttClient.publish(TOPIC_DIST, String(dist).c_str());
    String sensors = "L:" + String(leftVal) + " R:" + String(rightVal);
    mqttClient.publish(TOPIC_SENSORS, sensors.c_str());
    
   
    display.fillRect(0, 15, 128, 35, BLACK);
    display.setCursor(0, 15);
    display.print("Dist: "); display.print(dist); display.println(" cm");
    display.print("L: "); display.print(leftVal);
    display.print("  R: "); display.println(rightVal);
    display.display();
  }


  if (!isStopped) {
    if (dist < obstacleThreshold) {
      Serial.println("!!! OBSTACLE - TURNING !!!");
      digitalWrite(buzzerPin, HIGH); 
      turnRight();
    } else {
      digitalWrite(buzzerPin, LOW); 
      if (leftVal) turnLeft();
      else if (rightVal) turnRight();
      else moveForward();
    }
  } else {
     digitalWrite(buzzerPin, LOW);
     delay(100); 
  }
  
 
}