"""WRO Future Engineers - 绕圈赛"""

# =============================================================
# GPIO 后端必须在导入 buildhat 之前确定。
# buildhat 一导入就会用 gpiozero 操作 GPIO4 复位 HAT，
# 而 gpiozero 默认可能挑到 RPi.GPIO，在部分系统上报
# "Not running on a RPi!"。这里按可用性显式指定。
#
# pigpio 最省事，两种身份都能用：
#   sudo apt install -y pigpio python3-pigpio
#   sudo systemctl enable --now pigpiod
# =============================================================
import importlib
import os


def init_pin_factory():
    """依次尝试各后端并设为默认，返回成功的名字"""
    candidates = [
        ("lgpio", "gpiozero.pins.lgpio", "LGPIOFactory"),
        ("pigpio", "gpiozero.pins.pigpio", "PiGPIOFactory"),
        ("native", "gpiozero.pins.native", "NativeFactory"),
        ("rpigpio", "gpiozero.pins.rpigpio", "RPiGPIOFactory"),
    ]
    try:
        from gpiozero import Device
    except ImportError as exc:
        print(f"gpiozero 不可用: {exc}")
        return None

    for label, module, cls in candidates:
        try:
            Device.pin_factory = getattr(
                importlib.import_module(module), cls)()
            print(f"GPIO 后端: {label}")
            return label
        except Exception as exc:
            print(f"  {label} 不可用: {exc}")
    print("没有可用的 GPIO 后端，BuildHAT 将无法初始化")
    return None


init_pin_factory()

from buildhat import Motor, DistanceSensor
import cv2
import numpy as np
import time


# -------------------------------------------------------------
# 启动按钮：只需读取一个引脚的高低电平
# 依次尝试 libgpiod / sysfs / gpiozero，全部失败则退回键盘启动
# -------------------------------------------------------------
class SimpleButton:
    def __init__(self, pin, active_high=True, pull="pd"):
        self.pin = pin
        self.active_high = active_high
        self.mode = None
        self._req = self._line = self._file = self._gz = None

        errors = []
        for setup in (self._gpiod, self._sysfs, self._gpiozero):
            try:
                if setup(pull):
                    break
                errors.append(f"{setup.__name__}: 不可用")
            except Exception as exc:
                errors.append(f"{setup.__name__}: {exc}")

        if self.mode is None:
            raise RuntimeError(f"GPIO{pin} 读取失败 -> " + "; ".join(errors))
        print(f"GPIO{pin} 读取方式: {self.mode}")

    def _gpiod(self, pull):
        import gpiod
        if hasattr(gpiod, "request_lines"):                  # v2 API
            from gpiod.line import Bias, Direction
            bias = Bias.PULL_DOWN if pull == "pd" else Bias.PULL_UP
            self._req = gpiod.request_lines(
                "/dev/gpiochip0", consumer="wro",
                config={self.pin: gpiod.LineSettings(
                    direction=Direction.INPUT, bias=bias)})
            self.mode = "gpiod v2"
        else:                                                # v1 API
            line = gpiod.Chip("gpiochip0").get_line(self.pin)
            flag = (gpiod.LINE_REQ_FLAG_BIAS_PULL_DOWN if pull == "pd"
                    else gpiod.LINE_REQ_FLAG_BIAS_PULL_UP)
            try:
                line.request(consumer="wro",
                             type=gpiod.LINE_REQ_DIR_IN, flags=flag)
            except (AttributeError, OSError):
                line.request(consumer="wro", type=gpiod.LINE_REQ_DIR_IN)
            self._line = line
            self.mode = "gpiod v1"
        return True

    def _sysfs(self, pull):
        base = f"/sys/class/gpio/gpio{self.pin}"
        if not os.path.isdir("/sys/class/gpio"):
            raise RuntimeError("内核未提供 /sys/class/gpio")
        if not os.path.exists(base):
            with open("/sys/class/gpio/export", "w") as f:
                f.write(str(self.pin))
        with open(f"{base}/direction", "w") as f:
            f.write("in")
        self._file = open(f"{base}/value")
        # sysfs 无法配置片内上下拉，GPIO6 上电默认上拉，
        # 走到这条路径时必须外接 10k 下拉电阻到 GND
        self.mode = "sysfs (需外接下拉电阻)"
        return True

    def _gpiozero(self, pull):
        from gpiozero import Button
        self._gz = Button(self.pin, pull_up=(pull == "pu"), bounce_time=0.05)
        self.mode = "gpiozero"
        return True

    @property
    def level(self):
        if self._req is not None:
            from gpiod.line import Value
            return 1 if self._req.get_value(self.pin) == Value.ACTIVE else 0
        if self._line is not None:
            return int(self._line.get_value())
        if self._file is not None:
            self._file.seek(0)
            return int(self._file.read().strip() or 0)
        if self._gz is not None:
            return 1 if self._gz.is_active else 0
        return 0

    @property
    def is_pressed(self):
        return bool(self.level) == self.active_high


HAS_GPIO = True

# =========================
# 硬件
# =========================
steer_motor = Motor("A")
drive_motor = Motor("D")
left_sensor = DistanceSensor("B")
right_sensor = DistanceSensor("C")

steer_motor.release = False

# =========================
# 摄像头
# =========================
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)

# =========================
# 启动按钮
# 接线：3.3V(物理17) -- 660R -- 按钮 -- GPIO6(物理31)
#       片内下拉，按下读高
# Build HAT 占用 GPIO0/1/4/14/15/16/17，不要使用这些引脚
# =========================
BUTTON_ACTIVE_HIGH = True   # 按钮接 3.3V，按下为高电平；接 GND 则改 False
BUTTON_PULL = "pd"          # 片内下拉 pd / 上拉 pu，与上一行配套
BUTTON_PIN = 6           # BCM 编号，GPIO6 在物理第 31 脚
BUTTON_BOUNCE = 0.05
REQUIRE_BUTTON = True    # 桌面测试可设 False，改按键盘 s 启动


# =========================
# 电机与控制参数
# =========================
CENTER_POS = 0
MAX_STEER = 30

DRIVE_SPEED = -22
TURN_SPEED = -15

KP = 0.1
KD = 0.05
LOOP_DT = 0.05

# =========================
# 转弯参数
# =========================
TURN_START_BLACK = 500
TURN_END_BLACK = 200
MIN_TURN_TIME = 0.35
TURN_COOLDOWN = 0.8

START_CALIBRATE_TIME = 0.8
TURN_CALIBRATE_TIME = 0.4

SAFE_DISTANCE = 150

# =========================
# 颜色与画面参数
# =========================
LINE_THRESHOLD = 3000

WALL_ROI = (420, 240, 860, 480)
LINE_ROI = (250, 600, 1030, 720)

LOWER_BLACK = np.array([0, 0, 0])
UPPER_BLACK = np.array([180, 60, 120])

LOWER_ORANGE = np.array([5, 120, 120])
UPPER_ORANGE = np.array([25, 255, 255])

LOWER_BLUE = np.array([90, 100, 80])
UPPER_BLUE = np.array([130, 255, 255])


# =========================
# 显示开关
# 比赛时无显示器，设为 False；调试时设为 True
# 也可用环境变量覆盖：WRO_HEADLESS=1 python3 xxx.py
# =========================
SHOW_WINDOWS = os.environ.get("WRO_HEADLESS", "0") != "1"


def show(name, img):
    """显示一帧。空图跳过；显示后端不可用时自动转入无窗口模式"""
    global SHOW_WINDOWS
    if not SHOW_WINDOWS or img is None or getattr(img, "size", 0) == 0:
        return
    try:
        cv2.imshow(name, img)
    except cv2.error as exc:
        print(f"显示不可用，转入无窗口模式: {exc}")
        SHOW_WINDOWS = False


def wait_key(ms):
    """无窗口时用 sleep 代替，返回 -1 表示没有按键"""
    if SHOW_WINDOWS:
        return cv2.waitKey(ms) & 0xFF
    time.sleep(ms / 1000.0)
    return 255


# =========================
# 工具函数
# =========================
def clamp(value, low, high):
    return max(low, min(high, value))


def read_distance(sensor):
    value = sensor.get_distance()
    return 2000 if value is None or value < 0 else int(value)


def get_roi(frame, rect):
    x1, y1, x2, y2 = rect
    return frame[y1:y2, x1:x2]


last_steer_target = None


def steer_to(offset):
    global last_steer_target

    target = round(clamp(
        CENTER_POS + offset,
        CENTER_POS - MAX_STEER,
        CENTER_POS + MAX_STEER
    ))

    if target != last_steer_target:
        steer_motor.run_to_position(target, speed=100, blocking=False)
        last_steer_target = target


# =========================
# 状态变量
# =========================
state = "CALIBRATE"
calibrate_until = 0

ratio_samples = []
target_ratio = None

prev_error = 0
turn_direction = 0
turn_started_at = 0
last_turn_time = 0

corner_count = 0
quarter_count = 0
lap_count = 0
line_active = False

direction = None
target_color = None

# =========================
# 等待启动按钮
# 规则 9.11：开机后须处于等待状态，按下按钮才开始运行
# 等待期间画面照常刷新，方便上场前最后确认视觉与传感器
# =========================
start_button = None
if REQUIRE_BUTTON and HAS_GPIO:
    try:
        start_button = SimpleButton(
            BUTTON_PIN, active_high=BUTTON_ACTIVE_HIGH, pull=BUTTON_PULL
        )
    except Exception as exc:
        # 读不到引脚时不让程序崩掉，退回键盘启动
        print(f"Button init failed: {exc}")
        print("可尝试 sudo 运行，或 sudo usermod -aG gpio $USER 后重新登录")
        start_button = None
    edge = "HIGH" if BUTTON_ACTIVE_HIGH else "LOW"
    print(f"Waiting for button on GPIO{BUTTON_PIN} (pressed = {edge}) ...")
else:
    print("Button disabled, press 's' to start")

# 启动判定：必须先读到稳定的松开，再读到稳定的按下
STABLE_SAMPLES = 5       # 连续多少次采样一致才认，约 150ms
released_once = False
press_count = 0
release_count = 0
last_report = 0.0

steer_to(0)

while True:
    ok, frame = cap.read()
    if ok:
        cv2.putText(
            frame, "READY - press start button", (40, 80),
            cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 255, 255), 3
        )
        cv2.putText(
            frame,
            f"L:{read_distance(left_sensor)}  R:{read_distance(right_sensor)}",
            (40, 140), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2
        )
        show("Camera", cv2.resize(frame, (680, 480)))

    if start_button is not None:
        # 连续多次采样一致才认，单次跳变多半是引脚悬空造成的噪声
        if start_button.is_pressed:
            press_count += 1
            release_count = 0
        else:
            release_count += 1
            press_count = 0

        if release_count >= STABLE_SAMPLES:
            released_once = True

        if released_once and press_count >= STABLE_SAMPLES:
            break

        # 每隔约一秒打印一次引脚状态，无窗口模式下也能看出是否悬空
        if time.time() - last_report > 1.0:
            last_report = time.time()
            level = "HIGH" if start_button.is_pressed else "LOW"
            hint = "" if released_once else "  (尚未读到稳定的松开状态)"
            print(f"pin={level}{hint}")

    key = wait_key(30)
    if key == ord("s"):
        break
    if key == ord("q"):
        cap.release()
        cv2.destroyAllWindows()
        raise SystemExit("Aborted before start")

print("START")
time.sleep(0.3)
calibrate_until = time.time() + START_CALIBRATE_TIME
drive_motor.start(DRIVE_SPEED)

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

        now = time.time()

        # =========================
        # 颜色线识别
        # =========================
        hsv_line = cv2.cvtColor(get_roi(frame, LINE_ROI), cv2.COLOR_BGR2HSV)

        orange_pixels = cv2.countNonZero(
            cv2.inRange(hsv_line, LOWER_ORANGE, UPPER_ORANGE)
        )
        blue_pixels = cv2.countNonZero(
            cv2.inRange(hsv_line, LOWER_BLUE, UPPER_BLUE)
        )

        if direction is None:
            if blue_pixels > LINE_THRESHOLD:
                direction = "CCW"
                target_color = "BLUE"
                print("COUNTER CLOCKWISE")
            elif orange_pixels > LINE_THRESHOLD:
                direction = "CW"
                target_color = "ORANGE"
                print("CLOCKWISE")

        current_detect = (
            target_color == "BLUE" and blue_pixels > LINE_THRESHOLD
        ) or (
            target_color == "ORANGE" and orange_pixels > LINE_THRESHOLD
        )

        if current_detect and not line_active:
            quarter_count += 1
            print(f"Quarter = {quarter_count}")

        line_active = current_detect
        lap_count = max(quarter_count - 1, 0) // 4

        # =========================
        # 黑墙识别
        # =========================
        hsv_wall = cv2.cvtColor(get_roi(frame, WALL_ROI), cv2.COLOR_BGR2HSV)
        black_mask = cv2.inRange(hsv_wall, LOWER_BLACK, UPPER_BLACK)
        black_pixels = cv2.countNonZero(black_mask)

        # =========================
        # 超声波
        # =========================
        left = read_distance(left_sensor)
        right = read_distance(right_sensor)

        width = left + right
        raw_difference = left - right

        # =========================
        # CALIBRATE
        # =========================
        if state == "CALIBRATE":
            steer_to(0)

            if target_ratio is None and width > 0:
                ratio_samples.append(left / width)

            if now >= calibrate_until:
                if target_ratio is None and ratio_samples:
                    target_ratio = float(np.median(ratio_samples))
                    min_ratio = SAFE_DISTANCE / 600
                    target_ratio = clamp(target_ratio, min_ratio, 1 - min_ratio)
                    print(f"START RATIO = {target_ratio:.2f}")

                prev_error = 0
                state = "FOLLOW"

        # =========================
        # FOLLOW
        # =========================
        elif state == "FOLLOW":
            if target_ratio is not None and width > 0:
                min_ratio = min(SAFE_DISTANCE / width, 0.49)
                active_ratio = clamp(target_ratio, min_ratio, 1 - min_ratio)

                target_difference = (2 * active_ratio - 1) * width
                follow_error = raw_difference - target_difference
                derivative = (follow_error - prev_error) / LOOP_DT

                output = KP * follow_error + KD * derivative
                steer_to(clamp(output, -MAX_STEER, MAX_STEER))
                prev_error = follow_error

            if (
                black_pixels > TURN_START_BLACK
                and now - last_turn_time > TURN_COOLDOWN
            ):
                if direction == "CW":
                    turn_direction = -1
                elif direction == "CCW":
                    turn_direction = 1
                else:
                    turn_direction = 1 if raw_difference > 0 else -1

                print(f"TURN START: black={black_pixels}, dir={turn_direction}")

                drive_motor.start(TURN_SPEED)
                turn_started_at = now
                state = "TURN"

        # =========================
        # TURN
        # =========================
        elif state == "TURN":
            steer_to(turn_direction * MAX_STEER)

            if (
                now - turn_started_at > MIN_TURN_TIME
                and black_pixels < TURN_END_BLACK
            ):
                corner_count += 1
                last_turn_time = now
                print(f"TURN FINISHED {corner_count}")

                drive_motor.start(DRIVE_SPEED)
                steer_to(0)

                calibrate_until = now + TURN_CALIBRATE_TIME
                state = "CALIBRATE"

        # =========================
        # 画面显示
        # =========================
        channel_type = "NARROW" if width < 800 else "WIDE"

        x1, y1, x2, y2 = WALL_ROI
        roi_color = (
            (0, 255, 0) if black_pixels > TURN_START_BLACK else (0, 0, 255)
        )
        cv2.rectangle(frame, (x1, y1), (x2, y2), roi_color, 3)

        info = [
            (f"L:{left} R:{right}", (0, 255, 0)),
            (f"Width:{width} {channel_type}", (0, 255, 0)),
            (f"Black:{black_pixels}", (0, 255, 0)),
            (f"State:{state}", (0, 255, 255)),
            (f"Direction:{direction}", (0, 255, 255)),
            (f"Quarter:{quarter_count}", (0, 255, 255)),
            (f"Lap:{lap_count}", (0, 255, 255)),
            (f"Corners:{corner_count}", (0, 255, 255)),
        ]

        for i, (text, color) in enumerate(info):
            cv2.putText(
                frame, text, (20, 40 + i * 35),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, color, 2
            )

        show("Camera", cv2.resize(frame, (680, 480)))
        show("Black Wall", cv2.resize(black_mask, (340, 240)))

        # =========================
        # 结束条件
        # =========================
        if lap_count >= 3:
            print("3 LAPS FINISHED")
            break

        if wait_key(1) == ord("q"):
            print("QUIT")
            break

        time.sleep(LOOP_DT)

finally:
    drive_motor.stop()
    steer_to(0)
    cap.release()
    cv2.destroyAllWindows()

print("Finished")