import cv2
import numpy as np

def process_frame(frame):
    height, width = frame.shape[:2]
    roi = frame[int(height*2/3):height, :]  # 하단 1/3 영역만 사용

    gray = cv2.cvtColor(roi, cv2.COLOR_BGR2GRAY)
    blur = cv2.GaussianBlur(gray, (5, 5), 0)
    edges = cv2.Canny(blur, 50, 150)

    # 왼쪽, 오른쪽 나누기
    left_half = edges[:, :width//2]
    right_half = edges[:, width//2:]

    # 왼쪽 차선 찾기
    left_contours, _ = cv2.findContours(left_half.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    left_cx = None
    if left_contours:
        left_c = max(left_contours, key=cv2.contourArea)
        M = cv2.moments(left_c)
        if M['m00'] != 0:
            left_cx = int(M['m10']/M['m00'])

    # 오른쪽 차선 찾기
    right_contours, _ = cv2.findContours(right_half.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    right_cx = None
    if right_contours:
        right_c = max(right_contours, key=cv2.contourArea)
        M = cv2.moments(right_c)
        if M['m00'] != 0:
            right_cx = int(M['m10']/M['m00']) + width // 2  # 오른쪽은 전체 기준 좌표로 보정

    # 차선 중간점 계산
    if left_cx is not None and right_cx is not None:
        center_lane = (left_cx + right_cx) // 2
        car_center = width // 2
        error = center_lane - car_center

        if error > 30:
            print("Turn Right")
        elif error < -30:
            print("Turn Left")
        else:
            print("Go")
        
        # 디버그 시각화
        cv2.line(frame, (center_lane, height-10), (center_lane, height-50), (0,255,0), 3)
        cv2.line(frame, (car_center, height-10), (car_center, height-50), (255,0,0), 3)
    else:
        print("Lane not detected")

    return frame

if __name__ == "__main__":
    cap = cv2.VideoCapture(0)

    while cap.isOpened():
        ret, frame = cap.read()
        if not ret:
            break

        result = process_frame(frame)
        cv2.imshow("Lane Detection", result)

        if cv2.waitKey(1) == ord('q'):
            break

    cap.release()
    cv2.destroyAllWindows()