#include <Wire.h>
#include <SparkFun_TB6612.h>
#include "Adafruit_VL53L0X.h"

#define AIN1 D5
#define BIN1 D6
#define AIN2 D4
#define BIN2 D7
#define PWMA D3
#define PWMB D8
#define STBY D9

const int fwdDirectionA = 1;
const int fwdDirectionB = 1;

Motor motor1 = Motor(AIN1, AIN2, PWMA, fwdDirectionA, STBY);
Motor motor2 = Motor(BIN1, BIN2, PWMB, fwdDirectionB, STBY);

Adafruit_VL53L0X distanceSensor = Adafruit_VL53L0X();

#define SDA 21
#define SCL 22

unsigned long lastMeasureTime = 0;
const unsigned long measureInterval = 100;

void setup() {
  Serial.begin(115200);
  Wire.begin(SDA, SCL);

  if (!distanceSensor.begin()) {
    Serial.println("Failed to find distance sensor!");
    while (1) delay(10);
  }
  Serial.println("Distance sensor initialized.");
}

void loop() {
  unsigned long currentTime = millis();
  if (currentTime - lastMeasureTime >= measureInterval) {
    lastMeasureTime = currentTime;

    VL53L0X_RangingMeasurementData_t measure;
    distanceSensor.rangingTest(&measure, false);

    if (measure.RangeStatus != 4) {
      Serial.print("Distance (mm): ");
      int distance = measure.RangeMilliMeter;
      Serial.println(distance);

      if (distance > 100) {
        motor1.drive(150);
        motor2.drive(150);
      } else if (distance < 60) {
        motor1.drive(-150);
        motor2.drive(-150);
      } else {
        motor1.brake();
        motor2.brake();
      }
    } else {
      Serial.println("Out of range");
      motor1.drive(150);
      motor2.drive(150);
    }
  }
}