"""
[단계 8] P 제어로 차선 추종 자율주행

학습 목표:
  - 단계 7의 차선 인식 결과(error)를 조향각으로 변환
  - 비례(P) 제어: steering = 90 + Kp * error
  - UDP로 차량에 명령 송신

조작:
  s          : 자율주행 시작/정지 (안전을 위해 시작은 's' 키)
  + / -      : Kp 증가/감소
  w / x      : 속도 증가/감소
  SPACE      : 긴급 정지
  q          : 종료

⚠️ 안전 수칙
  1) 첫 실행은 차량 바퀴를 공중에 띄운 상태로
  2) 충분한 주행 공간 확보 (1.5m × 1.5m 이상)
  3) 항상 SPACE 키 위에 손가락
"""

import cv2
import json
import socket
import numpy as np

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

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

# HSV 흰색 차선
WHITE_LOWER = np.array([0,   0,   220], dtype=np.uint8)
WHITE_UPPER = np.array([180, 50,  255], dtype=np.uint8)

# 제어 파라미터 (실행 중 키로 조정 가능)
KP_INIT = 0.3
BASE_SPEED_INIT = 80

# 조향 범위
STEERING_MIN = 45
STEERING_MAX = 135
STEERING_NEUTRAL = 90


def clip(v, lo, hi):
    return max(lo, min(hi, v))


def detect_lane_center(frame):
    """
    프레임에서 차선 중심 x좌표를 반환.
    못 찾으면 None.

    반환: (cx, debug_mask)
    """
    h, w = frame.shape[:2]
    roi = frame[h // 2:, :]

    hsv = cv2.cvtColor(roi, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(hsv, WHITE_LOWER, WHITE_UPPER)

    kernel = np.ones((3, 3), np.uint8)
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel)

    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL,
                                    cv2.CHAIN_APPROX_SIMPLE)
    if not contours:
        return None, mask

    largest = max(contours, key=cv2.contourArea)
    M = cv2.moments(largest)
    if M['m00'] == 0:
        return None, mask

    cx = int(M['m10'] / M['m00'])
    return cx, mask


def calculate_steering(lane_cx, frame_center_x, kp):
    """
    P 제어: error = frame_center_x - lane_cx
    조향각 = 90 + Kp * error
    """
    if lane_cx is None:
        # 차선 못 찾음 → 직진 유지
        return STEERING_NEUTRAL

    # TODO 2: error 계산
    error = 0  # 이 줄을 수정

    # TODO 3: steering = 90 + Kp * error
    steering = STEERING_NEUTRAL  # 이 줄을 수정

    # TODO 4: STEERING_MIN ~ STEERING_MAX로 클립하고 정수 반환
    return STEERING_NEUTRAL  # 이 줄을 수정


def send_command(sock, speed, steering):
    cmd = {"speed": int(speed), "steering": int(steering)}
    sock.sendto(json.dumps(cmd).encode('utf-8'), (ESP32_IP, ESP32_PORT))


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

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    print("⚠️ 안전 확인 후 's' 키로 자율주행 시작")

    is_running = False
    kp = KP_INIT
    base_speed = BASE_SPEED_INIT

    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

        # 차선 검출
        cx, mask = detect_lane_center(frame)

        # 제어 계산
        steering = calculate_steering(cx, frame_center_x, kp)
        speed = base_speed if is_running else 0

        # 시각화
        if cx is not None:
            cv2.circle(frame, (cx, h // 2 + 30), 8, (0, 0, 255), -1)
        cv2.line(frame, (frame_center_x, 0), (frame_center_x, h),
                 (255, 0, 0), 1)

        status = "RUN" if is_running else "STOP"
        cv2.putText(frame, f"[{status}] Kp={kp:.2f} Spd={base_speed}",
                    (10, 25), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 2)
        cv2.putText(frame, f"Steering: {steering}",
                    (10, 50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,255), 1)

        cv2.imshow('Lane Following', frame)
        cv2.imshow('Mask', mask)

        # 송신
        send_command(sock, speed, steering)

        # 키 처리
        key = cv2.waitKey(1) & 0xFF
        if key == ord('q'):
            break
        elif key == ord('s'):
            is_running = not is_running
            print(f"자율주행: {'시작' if is_running else '정지'}")
        elif key == ord(' '):
            is_running = False
            send_command(sock, 0, STEERING_NEUTRAL)
            print("긴급 정지!")
        elif key in (ord('+'), ord('=')):
            kp += 0.05
            print(f"Kp = {kp:.2f}")
        elif key == ord('-'):
            kp = max(0.05, kp - 0.05)
            print(f"Kp = {kp:.2f}")
        elif key == ord('w'):
            base_speed = clip(base_speed + 10, 0, 200)
            print(f"speed = {base_speed}")
        elif key == ord('x'):
            base_speed = clip(base_speed - 10, 0, 200)
            print(f"speed = {base_speed}")

    # 정리
    send_command(sock, 0, STEERING_NEUTRAL)
    sock.close()
    cap.release()
    cv2.destroyAllWindows()
    print("종료")


if __name__ == "__main__":
    main()


# ════════════════════════════════════════════════════════════
# 정답
# ════════════════════════════════════════════════════════════
"""
[TODO 2] error = frame_center_x - lane_cx
[TODO 3] steering = STEERING_NEUTRAL + kp * error
[TODO 4]
    steering = STEERING_NEUTRAL + kp * error
    return int(clip(steering, STEERING_MIN, STEERING_MAX))
"""