/*
 * TinyBot Forge - ESP32 Obstacle-Avoiding Car v2 (four avoidance strategies)
 * https://tinybotforge.com/guides/esp32-obstacle-avoiding-robot-code/
 *
 * Hardware: ESP32 DevKit, HC-SR04 ultrasonic sensor, L298N motor driver, two DC gear motors.
 * Wiring:   same as https://tinybotforge.com/learn/obstacle-avoiding-car/
 * Toolchain: Arduino IDE 2.x or arduino-cli + "esp32 by Espressif Systems" 3.x
 *            (uses the 3.x ledcAttach / ledcWrite API).
 *
 * Pick a strategy below. 4 did best in our simulator (40 rooms, see the guide);
 * the Python twin obstacle_twin.py runs exactly the same logic.
 *
 * WARNING: a 5 V HC-SR04 drives ECHO at 5 V. ESP32 pins are 3.3 V only: use a
 *          1 kOhm + 2 kOhm divider on ECHO (not needed for the 3.3 V HC-SR04P).
 */

#ifndef STRATEGY
#define STRATEGY 4   // 1 back up + turn right | 2 random turn | 3 look both ways | 4 = 3 + stall watchdog
#endif

// ---------- Pins ----------
const int PIN_TRIG = 18;
const int PIN_ECHO = 19;   // through the divider
const int PIN_ENA = 25;    // left motor PWM  (remove the ENA jumper)
const int PIN_IN1 = 26;
const int PIN_IN2 = 27;
const int PIN_ENB = 33;    // right motor PWM (remove the ENB jumper)
const int PIN_IN3 = 32;
const int PIN_IN4 = 13;

// ---------- Tuning (same names in obstacle_twin.py) ----------
const int PWM_FREQ = 1000;
const int PWM_BITS = 8;              // speed 0-255
const int SPEED_CRUISE = 170;
const int SPEED_TURN = 160;
const float STOP_CM = 20.0;
const unsigned long BACK_MS = 350;
const unsigned long TURN_MS = 450;   // about 90 degrees on our car; tune on yours
const unsigned long LOOK_MS = 300;   // one "look" to the side
const int WEAVE = 0;                 // strategy 4: optional PWM swing while cruising (35 tested: no gain)
const unsigned long WEAVE_MS = 1600; // strategy 4: one left-right weave
const unsigned long STALL_MS = 1500; // strategy 4: reading frozen this long = pinned

// ---------- Motors ----------
void setMotor(int pinEn, int pinA, int pinB, int speed) {
  speed = constrain(speed, -255, 255);
  digitalWrite(pinA, speed > 0 ? HIGH : LOW);
  digitalWrite(pinB, speed < 0 ? HIGH : LOW);
  ledcWrite(pinEn, abs(speed));
}

void drive(int left, int right) {
  setMotor(PIN_ENA, PIN_IN1, PIN_IN2, left);
  setMotor(PIN_ENB, PIN_IN3, PIN_IN4, right);
}

void stopCar() { drive(0, 0); }

// ---------- Ultrasonic ----------
float readOnceCm() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  unsigned long us = pulseIn(PIN_ECHO, HIGH, 25000UL);  // 25 ms timeout, about 4 m
  if (us == 0) return 999.0;                             // no echo came back
  return us / 58.0;                                      // datasheet: us / 58 = cm
}

float readDistanceCm() {                                 // median of three pings
  float a = readOnceCm(); delay(10);
  float b = readOnceCm(); delay(10);
  float c = readOnceCm();
  if (a > b) { float t = a; a = b; b = t; }
  if (b > c) { float t = b; b = c; c = t; }
  if (a > b) { float t = a; a = b; b = t; }
  return b;
}

// ---------- Avoidance moves ----------
void backUp() {
  stopCar(); delay(100);
  drive(-SPEED_CRUISE, -SPEED_CRUISE); delay(BACK_MS);
}

void avoidTurnRight() {                 // strategy 1 (our original tutorial)
  backUp();
  drive(SPEED_TURN, -SPEED_TURN); delay(TURN_MS);
  stopCar(); delay(80);
  if (readDistanceCm() > STOP_CM * 1.5) return;
  drive(-SPEED_TURN, SPEED_TURN); delay(TURN_MS * 2);
  stopCar(); delay(80);
}

void avoidRandom() {                    // strategy 2
  backUp();
  int sgn = random(2) ? 1 : -1;
  drive(sgn * SPEED_TURN, -sgn * SPEED_TURN);
  delay(TURN_MS + random(TURN_MS));     // 1x to 2x a quarter turn
  stopCar(); delay(80);
}

void lookBothWays() {                   // strategies 3 and 4
  backUp();
  drive(-SPEED_TURN, SPEED_TURN); delay(LOOK_MS);        // look left
  stopCar(); delay(80);
  float left = readDistanceCm();
  drive(SPEED_TURN, -SPEED_TURN); delay(2 * LOOK_MS);    // look right
  stopCar(); delay(80);
  float right = readDistanceCm();
  if (max(left, right) < STOP_CM * 1.5) {                // boxed in: turn around
    drive(SPEED_TURN, -SPEED_TURN); delay(2 * TURN_MS);
  } else if (left > right) {                             // left was more open
    drive(-SPEED_TURN, SPEED_TURN); delay(2 * LOOK_MS);
  }                                                      // else: already facing right
  stopCar(); delay(80);
}

// ---------- Main ----------
float lastCm = 999.0;
unsigned long lastChange = 0;

void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_IN1, OUTPUT);
  pinMode(PIN_IN2, OUTPUT);
  pinMode(PIN_IN3, OUTPUT);
  pinMode(PIN_IN4, OUTPUT);
  ledcAttach(PIN_ENA, PWM_FREQ, PWM_BITS);
  ledcAttach(PIN_ENB, PWM_FREQ, PWM_BITS);
  stopCar();
  randomSeed(esp_random());
  Serial.printf("TinyBot obstacle car v2, strategy %d: starting in 2 s\n", STRATEGY);
  delay(2000);
  lastChange = millis();
}

void loop() {
  float cm = readDistanceCm();
  unsigned long now = millis();
  Serial.printf("%lu ms  distance %.1f cm\n", now, cm);

#if STRATEGY == 1
  if (cm < STOP_CM) avoidTurnRight(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#elif STRATEGY == 2
  if (cm < STOP_CM) avoidRandom(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#elif STRATEGY == 3
  if (cm < STOP_CM) lookBothWays(); else drive(SPEED_CRUISE, SPEED_CRUISE);
#else
  if (fabsf(cm - lastCm) > 1.0) { lastCm = cm; lastChange = now; }
  if (cm < STOP_CM || now - lastChange > STALL_MS) {     // too close, or pinned and blind
    if (cm >= STOP_CM) Serial.println("stall watchdog: reading frozen, backing out");
    lookBothWays();
    lastCm = 999.0; lastChange = millis();
  } else {                                               // WEAVE > 0 sweeps the 15 deg beam
    int w = (int)(WEAVE * sinf(2.0f * PI * (float)now / WEAVE_MS));
    drive(SPEED_CRUISE + w, SPEED_CRUISE - w);
  }
#endif
  delay(40);
}
