# 循行者 8 路 × 樂高官方 SPIKE App（教育版 Python）範例 1：循線＋十字路口停車
# Pathfinder 8-Way × LEGO SPIKE App (Education, Python) example 1: line following + stop at a crossing
# 接線：循行者 → C 孔；左輪 E、右輪 F / Wiring: Pathfinder → port C; left wheel E, right wheel F
# 方向：站在車後往前看，線在左＝負 / standing behind the robot: line on the left = negative
# 看不到線時細位置停在 −100 或 +100，車子會自己往那一側轉回去找線 / when the line is lost, the robot turns back toward the last side
# ===== 循行者 8 路 讀值小工具（樂高官方 SPIKE App 3 教育版 Python，照抄就好、不用改）=====
# ===== Pathfinder 8-Way reading helper (LEGO SPIKE App 3, Education, Python: copy as is) =====
# 循行者在 SPIKE App 裡是「顏色感應器」：顏色＝線寬、反射光＝粗位置＋8、原始紅＝細位置＋100。
# In the SPIKE App the Pathfinder is a colour sensor: colour = line width, reflection = position + 8, raw red = fine position + 100.
# 原始綠、藍放的是 8 路光值（壓成 4 bit），這個小工具不拆；要每一路的光值請改用 Pybricks。
# Raw green and blue hold the 8 channel readings (4 bits each); this helper does not split them. Use Pybricks for those.
# 站在車後往前看：線在左＝負、在右＝正 / seen from behind the robot: line on the left = negative
import color_sensor


class Line8App:
    def __init__(self, port_number):
        self.port = port_number
        self.b = [0] * 8                      # byte 0~7

    def update(self):
        """讀一次最新資料 / read once"""
        c = color_sensor.color(self.port)              # byte 0 = 線寬 / line width
        r = color_sensor.reflection(self.port)         # byte 1 = 粗位置＋8 / position + 8
        rgb = color_sensor.rgbi(self.port)             # 原始紅、綠、藍（各 2 byte）/ raw red, green, blue
        b = [int(c) & 255, int(r) & 255]
        for v in rgb[:3]:
            v = int(v) & 0xFFFF                        # 負數也換回 0~65535 / negatives back to 0~65535
            b += [v & 255, v >> 8]
        self.b = b
        return True

    def _w(self, i):
        # 原始紅（i=0）、綠（1）、藍（2）的 0~65535 / raw red, green, blue word
        return self.b[2 + 2 * i] | (self.b[3 + 2 * i] << 8)

    def pos8(self):
        """粗位置 −8~+8，0＝正中間；看不到線時停在最後離開的那一側 / position −8~+8, 0 = centre; keeps the last side when the line is lost"""
        return self.b[1] - 8

    def pos100(self):
        """細位置 −100~+100，循線用這個比較順 / fine position −100~+100, smoother for line following"""
        return self._w(0) - 100

    def width(self):
        """線寬 0~8：幾路踩在線上，0＝看不到線 / line width 0~8: channels on the line, 0 = no line"""
        return self.b[0]
# ===== 小工具到這裡結束 / end of helper =====
from hub import port, sound
import motor_pair
import runloop

line8 = Line8App(port.C)
BASE = 250        # 👉 可以改：前進速度（度／秒）/ forward speed (deg/s)
KP = 2            # 👉 可以改：轉彎靈敏度，車子左右甩就調小 / steering gain, lower it if the robot wobbles
CROSS = 6         # 👉 可以改：線寬到這麼寬就當十字路口 / this wide = a crossing


async def main():
    motor_pair.pair(motor_pair.PAIR_1, port.E, port.F)
    while True:
        line8.update()
        if line8.width() >= CROSS:                # 整排都踩到：停車嗶一聲 / whole row on the line: stop and beep
            motor_pair.stop(motor_pair.PAIR_1)
            await sound.beep(880, 300, 100)
            break
        error = line8.pos100()                    # 線在左＝負 → 左輪慢 → 往左轉 / left = negative → left wheel slower
        motor_pair.move_tank(motor_pair.PAIR_1, int(BASE + KP * error), int(BASE - KP * error))
        await runloop.sleep_ms(10)

runloop.run(main())