#include <WiFi.h>
#include <WebServer.h>

// WiFi Configuration
const char* ssid = "ESP32_Robot";
const char* password = "12345678";

WebServer server(80);

// Motor Pins
constexpr int DIR_A   = 1;
constexpr int STEP_A  = 2;
constexpr int EN_A    = 3;
constexpr int DIR_B   = 4;
constexpr int STEP_B  = 6;
constexpr int EN_B    = 5;

// Ultrasonic Sensor Pins - PERIPHERAL VISION SETUP (30° each side)
#define TRIG_FRONT_RIGHT 9
#define ECHO_FRONT_RIGHT 8
#define TRIG_FRONT_LEFT 7
#define ECHO_FRONT_LEFT 44

// Motor Control
String currentAction = "stop";
unsigned long lastCommandTime = 0;
const unsigned long COMMAND_TIMEOUT = 500;

// Sensor Data - PERIPHERAL VISION
int frontRightDistance = 777;
int frontLeftDistance = 777;

// Speed Control
int baseStepDelay = 400;
int currentSpeed = 20;
const int MIN_DELAY = 25;
const int MAX_DELAY = 1000;
int currentStepDelay = 400;

// Obstacle Avoidance
const int SLOW_DOWN_DISTANCE = 40;
const int STOP_DISTANCE = 10;
const int AVOID_START_DISTANCE = 20;   // Start avoiding at 20cm
const int AVOID_CLEAR_DISTANCE = 35;   // Stop avoiding when > 30cm (hysteresis)
bool obstacleDetected = false;

constexpr int STEPS_PER_LOOP = 10;

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println("\n=== ESP32 Robot Control v2.1 - Peripheral Vision ===");
  
  // Motor Pins Setup
  pinMode(STEP_A, OUTPUT);
  pinMode(DIR_A, OUTPUT);
  pinMode(EN_A, OUTPUT);
  pinMode(STEP_B, OUTPUT);
  pinMode(DIR_B, OUTPUT);
  pinMode(EN_B, OUTPUT);
  
  digitalWrite(EN_A, HIGH);
  digitalWrite(EN_B, HIGH);
  
  // Ultrasonic Sensor Pins Setup
  pinMode(TRIG_FRONT_RIGHT, OUTPUT);
  pinMode(ECHO_FRONT_RIGHT, INPUT);
  pinMode(TRIG_FRONT_LEFT, OUTPUT);
  pinMode(ECHO_FRONT_LEFT, INPUT);
  
  Serial.println("✓ Peripheral ultrasonic sensors ready");
  
  // WiFi Setup
  WiFi.setTxPower(WIFI_POWER_19_5dBm);
  WiFi.softAP(ssid, password, 1, 0, 4);
  delay(500);
  
  Serial.print("✓ AP IP: ");
  Serial.println(WiFi.softAPIP());
  
  // HTTP Endpoints
  server.on("/cmd", handleCommand);
  server.on("/sensors", handleSensors);
  server.on("/ping", handlePing);
  server.on("/speed", handleSpeed);
  
  server.begin();
  Serial.println("✓ Server ready!\n");
}

void loop() {
  server.handleClient();
  
  // Read sensors (every 400ms - reduced to avoid WiFi blocking)
  static unsigned long lastSensorRead = 0;
  if (millis() - lastSensorRead > 400) {
    frontRightDistance = getDistance(TRIG_FRONT_RIGHT, ECHO_FRONT_RIGHT);
    frontLeftDistance = getDistance(TRIG_FRONT_LEFT, ECHO_FRONT_LEFT);
    lastSensorRead = millis();
    
    // Check for obstacles (distance-based, not timer-based!)
    checkObstacles();
  }
  
  // Execute motor movement
  if (currentAction != "stop" && !obstacleDetected) {
    if (millis() - lastCommandTime > COMMAND_TIMEOUT) {
      currentAction = "stop";
      stopMotors();
    } else {
      for (int i = 0; i < STEPS_PER_LOOP; i++) {
        executeMotorAction();
      }
    }
  }
  
  // Status output
  static unsigned long lastStatus = 0;
  if (millis() - lastStatus > 3000) {
    Serial.print("[Status] Action: ");
    Serial.print(currentAction);
    Serial.print(" | Speed: ");
    Serial.print(currentSpeed);
    Serial.print("% | FR: ");
    Serial.print(frontRightDistance);
    Serial.print("cm | FL: ");
    Serial.print(frontLeftDistance);
    Serial.println("cm");
    lastStatus = millis();
  }
}

// ========== Obstacle Avoidance ==========

void checkObstacles() {
  obstacleDetected = false;
  currentStepDelay = baseStepDelay;  // RESET speed
  
  // Get closest obstacle
  int closestDistance = min(frontRightDistance, frontLeftDistance);
  if (closestDistance == 0 || closestDistance > 500) {
    closestDistance = 777;
  }
  
  // Only process during forward/avoid movements
  if (currentAction == "forward" || currentAction == "avoid-left" || currentAction == "avoid-right") {
    
    // === EMERGENCY STOP ===
    if (closestDistance < STOP_DISTANCE) {
      obstacleDetected = true;
      stopMotors();
      Serial.println("⚠️ EMERGENCY STOP!");
      return;
    }
    
    // === DISTANCE-BASED AVOIDING ===
    
    // Currently avoiding - check if we can return to forward
    if (currentAction == "avoid-left") {
      // Check if left obstacle is now clear (> 30cm = hysteresis)
      if (frontLeftDistance > AVOID_CLEAR_DISTANCE || frontLeftDistance == 777) {
        Serial.println("✓ Left obstacle cleared → returning to FORWARD");
        currentAction = "forward";
        enableMotors();  // Re-enable both motors
        lastCommandTime = millis();  // Reset timeout
      } else {
        Serial.print("→ Still avoiding left (");
        Serial.print(frontLeftDistance);
        Serial.println("cm)");
      }
    }
    else if (currentAction == "avoid-right") {
      // Check if right obstacle is now clear (> 30cm = hysteresis)
      if (frontRightDistance > AVOID_CLEAR_DISTANCE || frontRightDistance == 777) {
        Serial.println("✓ Right obstacle cleared → returning to FORWARD");
        currentAction = "forward";
        enableMotors();  // Re-enable both motors
        lastCommandTime = millis();  // Reset timeout
      } else {
        Serial.print("→ Still avoiding right (");
        Serial.print(frontRightDistance);
        Serial.println("cm)");
      }
    }
    // Currently forward - check if we need to start avoiding
    else if (currentAction == "forward") {
      // Left obstacle closer and < threshold → avoid RIGHT
      if (frontLeftDistance < AVOID_START_DISTANCE && 
          frontLeftDistance > 0 && 
          frontLeftDistance < frontRightDistance - 5) {  // 5cm difference to avoid flickering
        
        Serial.print("⚠️ LEFT obstacle at ");
        Serial.print(frontLeftDistance);
        Serial.println("cm → AVOIDING RIGHT");
        
        currentAction = "avoid-right";
        digitalWrite(EN_A, HIGH);  // STOP left motor
        digitalWrite(EN_B, LOW);   // KEEP right motor running
        lastCommandTime = millis();  // Reset timeout
      }
      // Right obstacle closer and < threshold → avoid LEFT
      else if (frontRightDistance < AVOID_START_DISTANCE && 
               frontRightDistance > 0 && 
               frontRightDistance < frontLeftDistance - 5) {  // 5cm difference
        
        Serial.print("⚠️ RIGHT obstacle at ");
        Serial.print(frontRightDistance);
        Serial.println("cm → AVOIDING LEFT");
        
        currentAction = "avoid-left";
        digitalWrite(EN_A, LOW);   // KEEP left motor running
        digitalWrite(EN_B, HIGH);  // STOP right motor
        lastCommandTime = millis();  // Reset timeout
      }
    }
    
    // === SLOW DOWN (applies to all forward/avoid states) ===
    if (closestDistance < SLOW_DOWN_DISTANCE) {
      float slowFactor = (float)closestDistance / SLOW_DOWN_DISTANCE;
      currentStepDelay = baseStepDelay * (2.0 - slowFactor);
    }
  }
  
  // === CHECK MANUAL TURNS ===
  else if (currentAction == "right") {
    if (frontRightDistance < STOP_DISTANCE && frontRightDistance > 0) {
      obstacleDetected = true;
      stopMotors();
      Serial.println("⚠️ Can't turn right!");
    }
  }
  else if (currentAction == "left") {
    if (frontLeftDistance < STOP_DISTANCE && frontLeftDistance > 0) {
      obstacleDetected = true;
      stopMotors();
      Serial.println("⚠️ Can't turn left!");
    }
  }
}

// ========== HTTP Handlers ==========

void handleCommand() {
  if (server.hasArg("action")) {
    String action = server.arg("action");
    String previousAction = currentAction;
    
    // CRITICAL: Don't override avoid maneuvers with forward commands!
    // If we're currently avoiding and receive "forward", just reset timeout
    if ((previousAction == "avoid-left" || previousAction == "avoid-right") && 
        action == "forward") {
      lastCommandTime = millis();  // Just reset timeout, keep avoiding!
      server.send(200, "text/plain", "OK");
      return;  // Don't change currentAction!
    }
    
    // Update action
    currentAction = action;
    lastCommandTime = millis();
    
    if (action == "stop") {
      stopMotors();
      obstacleDetected = false;
      Serial.println("→ Command: STOP");
    } else {
      // Only enable motors when starting from stop or changing manual direction
      if (previousAction == "stop") {
        enableMotors();
        Serial.println("→ Command: " + action);
      } else {
        Serial.println("→ Command: " + action + " (timeout reset)");
      }
    }
    
    server.send(200, "text/plain", "OK");
  } else {
    server.send(400, "text/plain", "Missing action");
  }
}

void handleSpeed() {
  if (server.hasArg("speed")) {
    currentSpeed = constrain(server.arg("speed").toInt(), 0, 100);
    baseStepDelay = map(currentSpeed, 0, 100, MAX_DELAY, MIN_DELAY);
    currentStepDelay = baseStepDelay;
    
    Serial.print("→ Speed: ");
    Serial.print(currentSpeed);
    Serial.print("% (");
    Serial.print(baseStepDelay);
    Serial.println("µs)");
    
    server.send(200, "text/plain", "OK");
  } else {
    server.send(400, "text/plain", "Missing speed");
  }
}

void handleSensors() {
  String json = "{\"frontLeft\":" + String(frontLeftDistance) + 
                ",\"frontRight\":" + String(frontRightDistance) + 
                ",\"speed\":" + String(currentSpeed) + "}";
  server.send(200, "application/json", json);
}

void handlePing() {
  server.send(200, "text/plain", "PONG");
}

// ========== Motor Control ==========

void executeMotorAction() {
  // IMPORTANT: Do NOT call enableMotors() here!
  // Motor enable/disable is controlled by checkObstacles() for avoid maneuvers
  
  if (currentAction == "forward") {
    // Check if we're in an avoid maneuver
    // Read current enable states to avoid sending steps to disabled motors
    int enA = digitalRead(EN_A);
    int enB = digitalRead(EN_B);
    
    digitalWrite(DIR_A, LOW);
    digitalWrite(DIR_B, HIGH);
    
    // Only send steps to enabled motors
    if (enA == LOW) digitalWrite(STEP_A, HIGH);
    if (enB == LOW) digitalWrite(STEP_B, HIGH);
    delayMicroseconds(currentStepDelay);
    if (enA == LOW) digitalWrite(STEP_A, LOW);
    if (enB == LOW) digitalWrite(STEP_B, LOW);
    delayMicroseconds(currentStepDelay);
    
  } else if (currentAction == "backward") {
    digitalWrite(DIR_A, HIGH);
    digitalWrite(DIR_B, LOW);
    motorStep();
    
  } else if (currentAction == "left") {
    digitalWrite(DIR_A, LOW);
    digitalWrite(DIR_B, LOW);
    motorStep();
    
  } else if (currentAction == "right") {
    digitalWrite(DIR_A, HIGH);
    digitalWrite(DIR_B, HIGH);
    motorStep();
    
  } else if (currentAction == "avoid-right") {
    // ONLY right motor (B) runs - left motor (A) is disabled in checkObstacles()
    // Direction: forward
    digitalWrite(DIR_B, HIGH);
    digitalWrite(STEP_B, HIGH);
    delayMicroseconds(currentStepDelay);
    digitalWrite(STEP_B, LOW);
    delayMicroseconds(currentStepDelay);
    
  } else if (currentAction == "avoid-left") {
    // ONLY left motor (A) runs - right motor (B) is disabled in checkObstacles()
    // Direction: forward
    digitalWrite(DIR_A, LOW);
    digitalWrite(STEP_A, HIGH);
    delayMicroseconds(currentStepDelay);
    digitalWrite(STEP_A, LOW);
    delayMicroseconds(currentStepDelay);
  }
}

void motorStep() {
  digitalWrite(STEP_A, HIGH);
  digitalWrite(STEP_B, HIGH);
  delayMicroseconds(currentStepDelay);
  digitalWrite(STEP_A, LOW);
  digitalWrite(STEP_B, LOW);
  delayMicroseconds(currentStepDelay);
}

void enableMotors() {
  digitalWrite(EN_A, LOW);
  digitalWrite(EN_B, LOW);
}

void stopMotors() {
  digitalWrite(EN_A, HIGH);
  digitalWrite(EN_B, HIGH);
  currentAction = "stop";
  Serial.println("← Motors stopped");
}

// ========== Ultrasonic Sensor ==========

int getDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  // Reduced timeout to 15ms (max ~2.5m range) to avoid blocking
  long duration = pulseIn(echoPin, HIGH, 15000);
  
  if (duration == 0) return 777;
  
  int distance = duration * 0.034 / 2;
  return distance;
}