"""
[단계 7] HSV 튜닝 + 차선 중심점 검출

학습 목표:
  - cv2.inRange로 흰색 차선 마스크 만들기
  - cv2.findContours + 모멘트로 무게중심 구하기
  - 트랙바로 HSV 값을 실시간 튜닝

조작:
  q : 종료
  HSV 튜닝 창의 트랙바를 움직여 흰 차선만 보이도록 조정

목표:
  영상에 빨간 원으로 차선 중심이 표시되고,
  화면 중심과의 가로 거리(error)가 화면에 출력되도록 한다.
"""

import cv2
import numpy as np

# ──────────────────────────────────────────
# 설정
# ──────────────────────────────────────────

# TODO 1: 본인 차량 IP로 변경
ESP32_IP = "192.168.137.74"
STREAM_URL = f"http://{ESP32_IP}:80/stream"

# 초기 HSV 값 (트랙바로 변경 가능)
INIT_WHITE_V_MIN = 220
INIT_WHITE_S_MAX = 50

# ROI: 영상의 아래쪽 50%만 처리
ROI_Y_START_RATIO = 0.5


def nothing(x):
    pass


def main():
    cap = cv2.VideoCapture(STREAM_URL)
    if not cap.isOpened():
        print("스트림 연결 실패")
        return

    # 트랙바 창 만들기
    cv2.namedWindow('HSV Tuning')
    cv2.createTrackbar('White V Min', 'HSV Tuning', INIT_WHITE_V_MIN, 255, nothing)
    cv2.createTrackbar('White S Max', 'HSV Tuning', INIT_WHITE_S_MAX, 255, nothing)

    while True:
        ret, frame = cap.read()
        if not ret:
            continue

        # 좌우 미러링
        frame = cv2.flip(frame, 1)
        h, w = frame.shape[:2]
        frame_center_x = w // 2

        # ROI: 하단 50%
        roi_y = int(h * ROI_Y_START_RATIO)
        roi = frame[roi_y:, :]

        # ── 1) HSV 변환 ──
        # TODO 2: roi를 HSV로 변환
        # 힌트: cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
        hsv = None  # 이 줄을 수정

        # ── 2) 트랙바에서 값 읽기 ──
        v_min = cv2.getTrackbarPos('White V Min', 'HSV Tuning')
        s_max = cv2.getTrackbarPos('White S Max', 'HSV Tuning')

        white_lower = np.array([0,   0,     v_min], dtype=np.uint8)
        white_upper = np.array([180, s_max, 255],   dtype=np.uint8)

        # ── 3) 마스크 ──
        # TODO 3: hsv에 cv2.inRange를 적용해 흰색 마스크 만들기
        mask = None  # 이 줄을 수정

        # ── 4) 모폴로지 (노이즈 제거) ──
        kernel = np.ones((3, 3), np.uint8)
        mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)

        # ── 5) 윤곽선 검출 + 무게중심 ──
        contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                        cv2.CHAIN_APPROX_SIMPLE)

        cx, cy = None, None
        if contours:
            # TODO 4: 가장 큰 윤곽선 선택
            # 힌트: max(contours, key=cv2.contourArea)
            largest = None  # 이 줄을 수정

            M = cv2.moments(largest)
            if M['m00'] > 0:
                cx = int(M['m10'] / M['m00'])
                cy = int(M['m01'] / M['m00'])

        # ── 6) 시각화 ──
        # ROI에 마스크와 중심점 그리기
        debug_roi = cv2.cvtColor(mask, cv2.COLOR_GRAY2BGR)
        if cx is not None:
            cv2.circle(debug_roi, (cx, cy), 8, (0, 0, 255), -1)
            # 화면 중심에 파란 세로선
            cv2.line(debug_roi, (frame_center_x, 0),
                     (frame_center_x, debug_roi.shape[0]),
                     (255, 0, 0), 2)

        # error 계산
        if cx is not None:
            error = frame_center_x - cx
            error_text = f"Error: {error}"
        else:
            error_text = "Error: lane not found"

        # 원본 + 디버그 ROI 위아래로 합치기
        # 원본의 상단 부분
        top = frame[:roi_y, :].copy()
        cv2.putText(top, error_text, (10, 25),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2)
        combined = np.vstack([top, debug_roi])

        cv2.imshow('Lane Detection', combined)

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

    cap.release()
    cv2.destroyAllWindows()


if __name__ == "__main__":
    main()


# ════════════════════════════════════════════════════════════
# 정답
# ════════════════════════════════════════════════════════════
"""
[TODO 2] hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
[TODO 3] mask = cv2.inRange(hsv, white_lower, white_upper)
[TODO 4] largest = max(contours, key=cv2.contourArea)
"""