import RPi.GPIO as GPIO
import time

# GPIO 핀 설정 (BCM 모드 사용)
# 실제 연결한 핀 번호에 맞게 수정하세요.
GPIO_TRIGGER = 23  # TRIG 핀
GPIO_ECHO = 24     # ECHO 핀 (전압 분배 회로를 거쳐 라즈베리 파이에 연결)

# GPIO 모드 설정: BCM 모드는 GPIO 번호를 사용합니다.
GPIO.setmode(GPIO.BCM)

# TRIG 핀을 출력으로, ECHO 핀을 입력으로 설정합니다.
GPIO.setup(GPIO_TRIGGER, GPIO.OUT)
GPIO.setup(GPIO_ECHO, GPIO.IN)

print("HC-SR04 초음파 센서 테스트 시작")
print("Ctrl+C를 눌러 종료하세요.")

def measure_distance():
    # TRIG 핀을 LOW로 설정하여 센서 안정화
    GPIO.output(GPIO_TRIGGER, False)
    time.sleep(0.000002) # 2 마이크로초 대기 (안정화)

    # TRIG 핀을 HIGH로 설정하여 10 마이크로초 동안 펄스 발생
    GPIO.output(GPIO_TRIGGER, True)
    time.sleep(0.00001)
    GPIO.output(GPIO_TRIGGER, False)

    # ECHO 핀이 HIGH가 될 때까지 기다립니다 (펄스 수신 시작 시간)
    pulse_start = time.time()
    while GPIO.input(GPIO_ECHO) == 0:
        pulse_start = time.time()
        # 타임아웃 방지: 너무 오래 기다리지 않도록
        if time.time() - pulse_start > 0.04: # 약 4미터까지 측정, 0.04초면 충분
            return 999 # 측정 실패 또는 너무 멀리 있음

    # ECHO 핀이 LOW가 될 때까지 기다립니다 (펄스 수신 종료 시간)
    pulse_end = time.time()
    while GPIO.input(GPIO_ECHO) == 1:
        pulse_end = time.time()
        # 타임아웃 방지
        if time.time() - pulse_end > 0.04:
            return 999

    # 펄스 지속 시간 계산
    pulse_duration = pulse_end - pulse_start

    # 거리 계산 (소리의 속도: 343m/s 또는 34300 cm/s)
    # 왕복 거리이므로 2로 나눔
    # cm 단위로 변환: (시간 * 34300) / 2
    distance = (pulse_duration * 34300) / 2

    # 거리를 소수점 둘째 자리까지 반올림
    distance = round(distance, 2)

    return distance

try:
    while True:
        dist = measure_distance()
        if dist == 999:
            print("측정 실패 또는 범위 밖")
        else:
            print(f"거리: {dist} cm")
        time.sleep(1) # 1초마다 측정

except KeyboardInterrupt:
    # Ctrl+C가 눌리면 GPIO 설정을 정리합니다.
    print("\n프로그램 종료 및 GPIO 정리.")
    GPIO.cleanup()