/*
  Arduino | coding-help
  Kat's Exercises and Questions
  katdotbrush — 12/21/2025 9:05 AM

  While the sensor senses something:
  position increases, and the servo will be moving towards 180
  While the sensor senses nothing:
  position decreases, and the servo will be going back towards 0
*/

#include <Servo.h>

// user constants
const unsigned long SERVO_SPD = 25; // smaller is faster
const int DIST_THRESHOLD = 30;
// pin constants
const int SERVO_PIN = 4;
const int CLOSE_LED_PIN = 5;
const int FAR_LED_PIN = 6;
const int ECHO_PIN = 7;
const int TRIG_PIN = 8;
// global variables
int position = 0;
int oldDistance = 0;
unsigned long lastUpdate = 0;

Servo servo;

int getDistance() {
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  long duration = pulseIn(ECHO_PIN, HIGH);
  int distance = ((int) (((duration * 0.0343) / 2.0) + 0.5));
  return distance;
}

void updateServo(bool direction)  {
  if ((millis() - lastUpdate) > SERVO_SPD)  {
    lastUpdate = millis();
    int increment = direction ? 1 : -1;
    position += increment;
    if (position >= 180) position = 180;
    if (position <= 0) position = 0;
    servo.write(position);
    //Serial.println(position);
  }
}

void setup() {
  Serial.begin(9600);
  servo.attach(SERVO_PIN);
  pinMode(ECHO_PIN, INPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(FAR_LED_PIN, OUTPUT);
  pinMode(CLOSE_LED_PIN, OUTPUT);
  servo.write(position);
}

void loop() {
  int distance = getDistance();

  // print distance if it changes by 3 or more cm
  if ((distance) > (oldDistance + 3) || (distance) < (oldDistance - 3))  {
    oldDistance = distance;
    Serial.print("Distance: ");
    Serial.print(distance);
    Serial.println(" cm");
  }

  if (distance <= DIST_THRESHOLD) {
    digitalWrite(FAR_LED_PIN, LOW);
    digitalWrite(CLOSE_LED_PIN, HIGH);
    updateServo(true);
  } else {
    digitalWrite(FAR_LED_PIN, HIGH);
    digitalWrite(CLOSE_LED_PIN, LOW);
    updateServo(false);
  }

}