#include <AccelStepper.h>
#include <NewPing.h>

// ===== STEPPER SETUP (4-wire driver) =====
AccelStepper stepperL(AccelStepper::FULL4WIRE, 8, 10, 9, 11);
AccelStepper stepperR(AccelStepper::FULL4WIRE, 4, 6, 5, 7);

// ===== HC-SR04 SETUP =====
#define MAX_DISTANCE 200    // cm
NewPing sonar(12, 13, MAX_DISTANCE);

// ===== LINE SENSORS =====
const int pinLineL      = A0;
const int pinLineC      = A2;
const int pinLineR      = A1;
const int lineThreshold = 800;      // <800 = sees black

// ===== TIMING & STATE =====
unsigned long lastMeasureTime   = 0;
const unsigned long measureInterval = 50;   // ms between pings

// hold “follow” (line/obstacle) speeds this long before wandering:
const unsigned long lineHoldTime   = 300;     // ms
unsigned long lastLineDetectTime   = 0;
float   lastSpeedL = 0, lastSpeedR = 0;

// wander only every this long:
const unsigned long wanderInterval = 3000;    // ms
unsigned long lastWanderTime       = 0;

int distanceCM = 999;

void setup() {
  Serial.begin(115200);
  while (!Serial);

  // Steppers: max speed & acceleration
  stepperL.setMaxSpeed(1000.0);
  stepperL.setAcceleration(500.0);
  stepperR.setMaxSpeed(1000.0);
  stepperR.setAcceleration(500.0);
}

void loop() {
  unsigned long now = millis();

  // 1) Ping sonar every 100 ms
  if (now - lastMeasureTime >= measureInterval) {
    lastMeasureTime = now;
    distanceCM = sonar.ping_cm();  // dist to obstacle 
    if (distanceCM == 0) distanceCM = 999; // safe check
  }

  // 2) Read raw values from line sensors
  int valL = analogRead(pinLineL);
  int valC = analogRead(pinLineC);
  int valR = analogRead(pinLineR);
  bool onL = (valL < lineThreshold);
  bool onC = (valC < lineThreshold);
  bool onR = (valR < lineThreshold);
  bool sawLine = (onL || onC || onR);

  // 3) Print LINES -> l/r/c
  Serial.print("Line L: ");
  Serial.print(valL);
  Serial.print("   C: ");
  Serial.print(valC);
  Serial.print("   R: ");
  Serial.println(valR);

  // 4) Compute target speeds
  float speedL = 0, speedR = 0;
  const float baseSpeed  = 300.0;            // straight speed
  const float turnFactor = 0.99;          // steering aggressiveness

  if (distanceCM <= 15) {
    // obstacle → pivot away
    speedL = speedR = 300; // rotate 
    lastLineDetectTime = now;
    lastSpeedL = speedL;
    lastSpeedR = speedR;
  }
  else if (sawLine) {
    // fresh line detection → follow immediately (always go toward the detected sensor)
    lastLineDetectTime = now;
    if (onL) {
      // left sensor sees line → turn left
      speedL =  baseSpeed * (1 - turnFactor);
      speedR = -baseSpeed * (1 + turnFactor);
    }
    else if (onR) {
      // right sensor sees line → turn right
      speedL =  baseSpeed * (1 + turnFactor);
      speedR = -baseSpeed * (1 - turnFactor);
    }
    else {
      // only center sees line → straight
      speedL =  baseSpeed;
      speedR = -baseSpeed;
    }
    lastSpeedL = speedL;
    lastSpeedR = speedR;
  }
  else if (now - lastLineDetectTime < lineHoldTime) {
    // within hold window → keep previous follow speeds
    speedL = lastSpeedL;
    speedR = lastSpeedR;
  }
  else {
    // no line & hold expired → wander, but only update every wanderInterval
    if (now - lastWanderTime >= wanderInterval) {
      lastWanderTime = now;
      int r = random(100); // choose random number 0..100 - if smaller than 60 go forward, ...
      if (r < 60) {
        // forward
        speedL =  350;
        speedR = -350;
      }
      else if (r> 60 && r < 75) {
        // pivot left
        speedL = 200;
        speedR = 200;
      }
      else {
        // pivot right
        speedL = -200;
        speedR = -200;
      }
    }
    else {
      // keep whatever wander speed was last set
      speedL = lastSpeedL;
      speedR = lastSpeedR;
    }
    lastSpeedL = speedL;
    lastSpeedR = speedR;
  }

  // 5) Apply speeds and step (prevent blocking)
  stepperL.setSpeed(speedL);
  stepperR.setSpeed(speedR);
  stepperL.runSpeed();
  stepperR.runSpeed();
}