"""
[단계 5] 키보드로 차량 수동 조종

학습 목표:
  - cv2.waitKey()로 키 입력 받기
  - speed/steering 값을 키 입력에 따라 변경
  - UDP로 연속 송신

조작:
  w/s     : 속도 ± 10
  a/d     : 조향각 ± 5
  1~9     : 속도 프리셋 (25, 50, 75, ..., 225)
  n       : 조향 중립 (90)
  SPACE   : 긴급 정지
  q       : 종료

⚠️ 안전 수칙
  - 첫 실행은 반드시 차량을 책 위에 올려 바퀴가 공중에 뜨게!
  - SPACE 키 위에 항상 손가락 대기
"""

import socket
import json
import cv2
import numpy as np

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

# TODO 1: ESP32_IP를 본인 차량 IP로 변경
ESP32_IP = "192.168.137.74"
ESP32_PORT = 4210

STEERING_MIN = 45
STEERING_MAX = 135
STEERING_NEUTRAL = 90
SPEED_MAX = 220


# ──────────────────────────────────────────
# 함수
# ──────────────────────────────────────────

def send_command(sock, speed, steering):
    """현재 속도·조향각을 ESP32에 송신."""
    cmd = {"speed": speed, "steering": steering}
    data = json.dumps(cmd).encode('utf-8')
    sock.sendto(data, (ESP32_IP, ESP32_PORT))


def clip(value, lo, hi):
    """값을 [lo, hi] 범위로 제한."""
    # TODO 2: value가 lo보다 작으면 lo, hi보다 크면 hi, 그 외에는 value를 반환
    pass


def draw_status(speed, steering):
    """상태를 보여주는 검은 화면 만들기."""
    canvas = np.zeros((300, 500, 3), dtype=np.uint8)
    cv2.putText(canvas, f"Speed:    {speed}",   (20, 60),
                cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 0), 2)
    cv2.putText(canvas, f"Steering: {steering}", (20, 110),
                cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2)
    cv2.putText(canvas, "w/s: speed  a/d: steer", (20, 180),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1)
    cv2.putText(canvas, "SPACE: stop  q: quit",   (20, 210),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (200, 200, 200), 1)
    return canvas


# ──────────────────────────────────────────
# 메인
# ──────────────────────────────────────────

def main():
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    print(f"ESP32 대상: {ESP32_IP}:{ESP32_PORT}")
    print("⚠️ 안전: 차량 바퀴를 공중에 띄운 상태에서 시작하세요.")

    speed = 0
    steering = STEERING_NEUTRAL

    while True:
        canvas = draw_status(speed, steering)
        cv2.imshow('Manual Control', canvas)

        key = cv2.waitKey(50) & 0xFF

        if key == ord('q'):
            break

        elif key == ord(' '):
            # 긴급 정지
            speed = 0
            steering = STEERING_NEUTRAL

        elif key == ord('w'):
            # TODO 3: speed를 10 증가시키고 SPEED_MAX로 제한
            pass

        elif key == ord('s'):
            # TODO 4: speed를 10 감소시키고 -SPEED_MAX로 제한
            pass

        elif key == ord('a'):
            # TODO 5: steering을 5 감소시키고 STEERING_MIN으로 제한
            pass

        elif key == ord('d'):
            # TODO 6: steering을 5 증가시키고 STEERING_MAX로 제한
            pass

        elif key == ord('n'):
            steering = STEERING_NEUTRAL

        elif ord('1') <= key <= ord('9'):
            # 1~9 키 → 25, 50, ..., 225
            num = key - ord('0')
            speed = num * 25

        # 매 루프마다 ESP32에 송신
        send_command(sock, speed, steering)

    # 종료 시 정지 명령
    send_command(sock, 0, STEERING_NEUTRAL)
    sock.close()
    cv2.destroyAllWindows()
    print("종료")


if __name__ == "__main__":
    main()


# ════════════════════════════════════════════════════════════
# 정답
# ════════════════════════════════════════════════════════════
"""
[TODO 2]
def clip(value, lo, hi):
    if value < lo: return lo
    if value > hi: return hi
    return value

[TODO 3] speed = clip(speed + 10, -SPEED_MAX, SPEED_MAX)
[TODO 4] speed = clip(speed - 10, -SPEED_MAX, SPEED_MAX)
[TODO 5] steering = clip(steering - 5, STEERING_MIN, STEERING_MAX)
[TODO 6] steering = clip(steering + 5, STEERING_MIN, STEERING_MAX)
"""