// Simple 2WD Obstacle-Avoiding Robot (Arduino Nano/Uno) — Wokwi friendly
// - HC-SR04 on D3/D4
// - Button on D2 toggles RUN/PAUSE (INPUT_PULLUP)
// - Motor outputs mapped to LEDs in Wokwi so you can see direction & PWM
//   Left:  ENA D5 (PWM), IN1 D7, IN2 D8
//   Right: ENB D6 (PWM), IN3 D9, IN4 D10
// - Buzzer D11 (optional)
// Behavior: Drive forward; if obstacle <= threshold, reverse briefly then turn
//           Alternate turn direction each time for variety (left/right).

#define TRIG 3
#define ECHO 4
#define BTN  2
#define LED  13
#define BUZZ 11

// Motor pins
#define ENA 5
#define IN1 7
#define IN2 8
#define ENB 6
#define IN3 9
#define IN4 10

// Tuning
int SPEED_FWD = 180;     // 0..255
int SPEED_TURN = 180;    // 0..255
int THRESH_CM = 28;      // stop/avoid distance
unsigned long REVERSE_MS = 300;
unsigned long TURN_MS    = 420;

// State machine
enum State { PAUSED, FORWARD, REVERSE, TURN } state = PAUSED;
unsigned long stateStart = 0;
bool turnLeft = true;    // alternate each obstacle

// Button debounce
bool btnStable = HIGH, btnLast = HIGH;
unsigned long btnLastChange = 0;
const unsigned long DEBOUNCE_MS = 25;

// Helper: set both motors with signed speed (-255..255)
void setMotors(int left, int right){
  // Left
  if (left >= 0){ digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);  analogWrite(ENA, left); }
  else          { digitalWrite(IN1, LOW);  digitalWrite(IN2, HIGH); analogWrite(ENA, -left); }
  // Right
  if (right >= 0){ digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);  analogWrite(ENB, right); }
  else           { digitalWrite(IN3, LOW);  digitalWrite(IN4, HIGH); analogWrite(ENB, -right); }
}

// Helper: stop/brake
void brake(){
  analogWrite(ENA, 0); analogWrite(ENB, 0);
  digitalWrite(IN1, LOW); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, LOW);
}

// Simple blocking distance read (OK for this simple demo)
long readDistanceCM(){
  digitalWrite(TRIG, LOW); delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long duration = pulseIn(ECHO, HIGH, 30000UL); // timeout ~30ms
  if (duration == 0) return 999; // treat timeout as "far"
  return duration / 58; // us to cm
}

void beep(int f=1200, int ms=80){
  tone(BUZZ, f); delay(ms); noTone(BUZZ);
}

void enter(State s){
  state = s;
  stateStart = millis();
}

void setup(){
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  pinMode(BTN, INPUT_PULLUP);
  pinMode(LED, OUTPUT);
  pinMode(BUZZ, OUTPUT);

  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  brake();

  Serial.begin(115200);
  Serial.println("Simple 2WD Robot ready. Type 'start' or press the button.");
  enter(PAUSED);
}

void loop(){
  // --- CLI (very small) ---
  if (Serial.available()){
    String line = Serial.readStringUntil('\n'); line.trim();
    if (line=="start"){ if (state==PAUSED){ enter(FORWARD); beep(1320,60);} }
    else if (line=="stop"){ enter(PAUSED); brake(); beep(880,60); }
  }

  // --- Button debounce + toggle ---
  bool r = digitalRead(BTN);
  if (r != btnLast){ btnLastChange = millis(); btnLast = r; }
  if ((millis() - btnLastChange) > DEBOUNCE_MS && r != btnStable){
    btnStable = r;
    if (btnStable == LOW){
      if (state==PAUSED){ enter(FORWARD); beep(1320,60); }
      else { enter(PAUSED); brake(); beep(880,60); }
    }
  }

  // --- Heartbeat ---
  static unsigned long hb=0; if (millis()-hb>500){ hb=millis(); digitalWrite(LED, !digitalRead(LED)); }

  // --- Behavior ---
  switch(state){
    case PAUSED:
      brake();
      break;

    case FORWARD:{
      setMotors(SPEED_FWD, SPEED_FWD);
      long d = readDistanceCM();
      if (d <= THRESH_CM){
        enter(REVERSE);
      }
    } break;

    case REVERSE:{
      setMotors(-SPEED_FWD, -SPEED_FWD);
      if (millis() - stateStart >= REVERSE_MS){
        enter(TURN);
        turnLeft = !turnLeft; // alternate
      }
    } break;

    case TURN:{
      int v = SPEED_TURN;
      if (turnLeft) setMotors(-v, v);
      else          setMotors(v, -v);
      if (millis() - stateStart >= TURN_MS){
        enter(FORWARD);
      }
    } break;
  }
}