#include <WiFi.h>
#include <WiFiClient.h>
#include <PubSubClient.h>

// --- WIFI CONFIGURATION ---
#define WIFI_SSID "Wokwi-GUEST"
#define WIFI_PASSWORD ""
#define WIFI_CHANNEL 6

// --- MQTT SERVER (Updated with your Ngrok details) ---
const char *MQTT_SERVER = "0.tcp.ap.ngrok.io"; 
const uint16_t MQTT_PORT = 16180;

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

WiFiClient espClient; 
PubSubClient mqttClient(espClient); 

// --- PINS ---
const int trigPin = 5;
const int echoPin = 18;
const int leftSensorPin = 25; 
const int rightSensorPin = 26; 
const int stepPin1 = 19;
const int dirPin1 = 21;
const int stepPin2 = 22;
const int dirPin2 = 23;

const int obstacleThreshold = 20; 
bool isStopped = false; 
unsigned long lastTelemetry = 0; 

// --- MOTOR FUNCTIONS ---
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;
}

// --- MQTT FUNCTIONS ---
void Callback(char* topic, byte* payload, unsigned int length) {
  String message = "";
  for (int i = 0; i < length; i++) message += (char)payload[i];
  Serial.print("CMD Received: "); Serial.println(message);
  
  if (String(topic) == TOPIC_CMD) {
    if (message == "STOP") isStopped = true;
    else if (message == "GO") isStopped = false;
  }
}

void ConnectMqtt() {
  while (!mqttClient.connected()) {
    Serial.print("Connecting to MQTT...");
    if (mqttClient.connect(MQTT_CLIENT_NAME)) {
      Serial.println("Connected!");
      mqttClient.subscribe(TOPIC_CMD); 
    } else {
      Serial.print("Failed, rc="); Serial.print(mqttClient.state());
      Serial.println(" Retrying in 5s...");
      delay(5000);
    }
  }
}

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

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

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

void loop() {
  if (!mqttClient.connected()) ConnectMqtt();
  mqttClient.loop();

  // Send Data every 2 seconds
  if (millis() - lastTelemetry > 2000) {
    lastTelemetry = millis();
    int dist = getDistance();
    
    // Publish Distance
    mqttClient.publish(TOPIC_DIST, String(dist).c_str());
    
    // Publish Sensors
    String sensors = "L:" + String(digitalRead(leftSensorPin)) + " R:" + String(digitalRead(rightSensorPin));
    mqttClient.publish(TOPIC_SENSORS, sensors.c_str());
  }

  // Robot Logic
  if (!isStopped) {
    int dist = getDistance();
    if (dist < obstacleThreshold) turnRight();
    else if (digitalRead(leftSensorPin)) turnLeft();
    else if (digitalRead(rightSensorPin)) turnRight();
    else moveForward();
  }
}