/*
  Arduino | hardware-help
  Isabellewht — 11/1/24 at 10:47 AM
  https://hatebin.com/jfaqpqsdoe
  https://www.instagram.com/reel/CndgrXijILk/?igsh=dzhjM2phbWN0cjdm
*/

#include <Servo.h>

#define ECHO_PIN 8
#define TRIG_PIN 9
#define LED_PIN 7
#define SENSE_PIN 6
#define SERVO_PIN 5

int direction = 0;
int heart = 0;
int position = 0;
long duration = 0;
unsigned long lastTime = 0;
unsigned long oldDuration = 0;

Servo armServo;

void setup() {
  Serial.begin(9600);
  pinMode(LED_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(SENSE_PIN, INPUT);
  armServo.attach(SERVO_PIN);
}

void loop() {

  if (digitalRead(SENSE_PIN) == HIGH) {
    if (heart == 0) {
      digitalWrite(LED_PIN, HIGH);
    }
    if (heart == 1) {
      digitalWrite(LED_PIN, LOW);
    }
    delay(100);
    heart = !heart;
  }

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  duration = pulseIn(ECHO_PIN, HIGH, 2500);

  if (duration != oldDuration)  {
    oldDuration = duration;
    Serial.println(duration);
  }

  if (duration != 0 && duration > 500) {
    lastTime = millis();
  }

  if (millis() - lastTime > 400) {
    if (position != 120 && direction == 0) {
      armServo.write(position);
      position ++;
    }
    if (position == 120) {
      direction = 1;
    }
    if (position != 0 && direction == 1) {
      armServo.write(position);
      position--;
    }
    if (position == 0) {
      direction = 0;
    }
  }

}