// Ultrasonic (HC-SR04)
const int trigPin = 2;
const int echoPin = 7;

// Motors (PWM pins!)
const int motorPin = 10;

unsigned long detectStartTime = 0;
bool detecting = false;

float readDistanceCm(int trig, int echo) {
  // Send 10us trigger pulse
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  // Read echo pulse length (microseconds)
  long duration = pulseIn(echo, HIGH, 10000); // 10ms timeout (~170cm)

  if (duration == 0) return -1;
  return (duration * 0.0343) / 2.0;
}

void setup() {
  Serial.begin(9600);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  pinMode(motorPin, OUTPUT);

  Serial.println("Started!");
}

void loop() {
  float cm = readDistanceCm(trigPin, echoPin);

  Serial.print("Raw distance cm = ");
  Serial.println(cm);

  const float detectDistance = 80;
  int pwm1 = 0;
  int pwm2 = 0;

  if (cm > 0 && cm < detectDistance) {
    if (!detecting) {
      detecting = true;
      detectStartTime = millis();
    }

    unsigned long timeDetected = millis() - detectStartTime;
    if (timeDetected > 7000) timeDetected = 7000;

    // Power ramps up the longer it's detected
    pwm1 = map((int)timeDetected, 0, 7000, 0, 200);

    // Power stronger when closer (with guards)
    if (cm <= 10) {
      pwm2 = 55;
    } else if (cm >= 80) {
      pwm2 = 0;
    } else {
      pwm2 = map((int)cm, 10, 80, 55, 0);
    }

    Serial.print("Detected for ms = ");
    Serial.println(timeDetected);

  } else {
    detecting = false;
    pwm1 = 0;
    pwm2 = 0;
  }
  int pwmcombined = pwm1 + pwm2;
  analogWrite(motorPin, pwmcombined);

  Serial.print("PWM1 = ");
  Serial.println(pwm1);
  Serial.print("PWM2 = ");
  Serial.println(pwm2);
  Serial.print("PWMCOMB = ");
  Serial.println(pwmcombined);

  Serial.println("-----");

  delay(200);
}