SPIKE 範例：神攝手

# 神攝手 × Pybricks（SPIKE）範例 1：跟著色塊轉
# SharpShot × Pybricks (SPIKE) example 1: turn to follow a colour blob
# 需要把相機選單切到 相機模式 1 顏色追蹤 ・子選項 ③ 標準（XY＋面積）＝模式編號 13
# Camera menu: camera mode 1 Colour tracking, option 3 Standard (XY + area) = mode number 13
# 準備：先在相機上教「目標 1」的顏色 / Teach "target 1" on the camera first
# 接線 Wiring：相機 → C 孔 / camera → port C；左輪 E、右輪 F / left wheel E, right wheel F
# 同一個資料夾要有 MBC_UniCam.py / MBC_UniCam.py must be imported too
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction, Color
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from MBC_UniCam import UniCam

hub = PrimeHub()
cam = UniCam(Port.C, mode=13)                    # 函式庫會自動把相機切到這個模式 / the library switches the camera to this mode
left = Motor(Port.E, Direction.COUNTERCLOCKWISE)
right = Motor(Port.F)
robot = DriveBase(left, right, 56, 112)          # 輪徑 56 mm、輪距 112 mm / wheel 56 mm, axle 112 mm

KP = 3            # 👉 可以改：偏 1 像素就多轉幾度／秒 / you can change: turn rate per pixel of error
STOP_AREA = 20    # 👉 可以改：面積 ≥ 20 % 就算很近 / area (%) that counts as "close"

while True:
    p = cam.xy(1)                                # 色 1 的 (X, Y) 像素；(0, 0) = 沒看到 / (0, 0) = not seen
    if p is None:                                # 從來沒讀到相機 / camera never answered
        robot.stop()
        hub.light.on(Color.RED)
    elif p[0] == 0:                              # 沒看到色 1：原地慢慢轉著找 / not seen: spin slowly to search
        robot.drive(0, 60)
        hub.light.on(Color.ORANGE)
    else:
        error = p[0] - 80                        # 畫面中間 X = 80；色塊在右 → 正 / centre is X = 80; right = positive
        if cam.area(1) >= STOP_AREA:             # 很近了：只轉不前進 / close: turn only
            robot.drive(0, error * KP)
        else:                                    # 還遠：邊前進邊轉 / far: drive and turn
            robot.drive(80, error * KP)
        hub.light.on(Color.GREEN)
    wait(20)

# 神攝手 × Pybricks（SPIKE）範例 2：多色找位置
# SharpShot × Pybricks (SPIKE) example 2: where are my colours?
# 需要把相機選單切到 相機模式 1 顏色追蹤 ・子選項 ② 多色 ＝模式編號 12（SPIKE 給 7 色的 X、Y）
# Camera menu: camera mode 1 Colour tracking, option 2 Multi = mode number 12 (7 colours, X and Y)
# 準備：在相機上教目標 1～7 的顏色（教幾色都可以）/ teach targets 1~7 (any number)
# 接線 Wiring：相機 → C 孔 / camera → port C
from pybricks.hubs import PrimeHub
from pybricks.parameters import Port, Button
from pybricks.tools import wait
from MBC_UniCam import UniCam

hub = PrimeHub()
cam = UniCam(Port.C, mode=12)


def where(x, y):
    # 像素 → 文字：左／中／右 + 上／中／下 / pixels -> words
    h = '左 left' if x < 54 else ('右 right' if x > 106 else '中 middle')
    v = '上 top' if y < 40 else ('下 bottom' if y > 80 else '中 middle')
    return h + ', ' + v


while True:
    seen = 0
    for c in range(1, cam.max_color() + 1):      # 色 1～7 / colours 1~7
        p = cam.xy(c)
        if p and p[0]:                           # X = 0 表示沒找到 / X = 0 means not found
            seen += 1
            print('色 colour', c, ':', p[0], p[1], where(p[0], p[1]))
    hub.display.number(seen)                     # 主機螢幕顯示看到幾色 / show how many colours are seen
    if Button.LEFT in hub.buttons.pressed():
        # 數量放在模式 6，要多等約 30 ms，所以按鍵才讀 / counts are in mode 6 (30 ms slower): read on demand
        print('16 色裡找到', cam.colors_found(), '色 / colours found;  色 1 有', cam.count(1), '塊 / blobs of colour 1')
    print('---')
    wait(500)

# 神攝手 × Pybricks（SPIKE）範例 3：座標取色判斷顏色
# SharpShot × Pybricks (SPIKE) example 3: what colour is at the spot?
# 需要把相機選單切到 相機模式 2 座標取色 ・子選項 ② 色相＋飽和 ＝模式編號 22（SPIKE 給 7 點）
# Camera menu: camera mode 2 Spot colour, option 2 Hue + saturation = mode number 22 (7 spots)
# 準備：取樣點 S1 的位置看相機螢幕上標 1 的十字 / spot S1 is the cross marked 1 on the camera screen
# 接線 Wiring：相機 → C 孔 / camera → port C
from pybricks.hubs import PrimeHub
from pybricks.parameters import Port, Color
from pybricks.tools import wait
from MBC_UniCam import UniCam, color_name

hub = PrimeHub()
cam = UniCam(Port.C, mode=22)

# 顏色名稱 → 主機燈號 / colour name -> hub light
LIGHT = {'red': Color.RED, 'orange': Color.ORANGE, 'yellow': Color.YELLOW, 'green': Color.GREEN,
         'cyan': Color.CYAN, 'blue': Color.BLUE, 'purple': Color.VIOLET, 'white': Color.WHITE, 'black': Color.NONE}
NAME = {'red': '紅', 'orange': '橙', 'yellow': '黃', 'green': '綠', 'cyan': '青',
        'blue': '藍', 'purple': '紫', 'white': '白／灰', 'black': '黑'}
LOW_SAT = 15      # 👉 可以改：飽和度比這低就當成白／灰 / below this saturation = white / grey

while True:
    p = cam.hsv(1)                               # 取樣點 S1 的 (色相碼, 飽和度, None) / (hue, sat, None)
    if p is None:                                # 讀不到相機 / no camera
        hub.light.on(Color.NONE)
        wait(200)
        continue
    h, s = p[0], p[1]
    name = color_name(h)
    if s < LOW_SAT and name != 'black':
        name = 'white'                           # 顏色很淡：當成白／灰 / very pale: white / grey
    hub.light.on(LIGHT[name])
    print('S1 色相碼 hue', h, ' 飽和度 sat', s, ' →', NAME[name], name)
    wait(200)

# 神攝手 × Pybricks（SPIKE）範例 4：照順序給色號（WRO 任務：記下積木的顏色順序）
# SharpShot × Pybricks (SPIKE) example 4: colour order (WRO: remember the order)
# 需要把相機選單切到 相機模式 3 照順序給色號（沒有子選項）＝模式編號 30
# Camera menu: camera mode 3 Colour order (no option) = mode number 30
# 準備：在相機上教會場地上會出現的每一種顏色 / teach every colour that can appear on the field
# 接線 Wiring：相機 → C 孔 / camera → port C
from pybricks.hubs import PrimeHub
from pybricks.parameters import Port
from pybricks.tools import wait
from MBC_UniCam import UniCam, color_name

hub = PrimeHub()
cam = UniCam(Port.C, mode=30)

WANT = 4          # 👉 可以改：場地上有幾塊 / how many blocks to expect
LETTER = {'red': 'R', 'orange': 'O', 'yellow': 'Y', 'green': 'G', 'cyan': 'C',
          'blue': 'B', 'purple': 'P', 'white': 'W', 'black': 'K'}

# 連續 5 次讀到一樣的順序才算數（避免剛好有手經過）/ same answer 5 times in a row = stable
last = None
same = 0
names = []
while same < 5:
    hues = cam.order()                           # 由左到右每塊的色相碼 / hue codes from left to right
    names = [color_name(h) for h in hues] if hues else []
    if len(names) == WANT and names == last:
        same += 1
    else:
        same = 0
    last = names
    wait(50)

print('順序 order:', names)
hub.speaker.beep()
for n in names:                                  # 一個字母一個字母顯示 / show one letter at a time
    hub.display.char(LETTER[n])
    wait(700)
hub.display.off()

# 神攝手 × Pybricks（SPIKE）範例 5：PD 循線＋路口停車
# SharpShot × Pybricks (SPIKE) example 5: PD line following + stop at a junction
# 需要把相機選單切到 相機模式 4 循線 ・子選項 ① 黑線 ＝模式編號 41（白線 42、色線 43 寫法一樣，改 mode）
# Camera menu: camera mode 4 Line, option 1 Black line = mode number 41 (42 white / 43 colour line: change mode)
# 準備：相機裝在車頭、斜斜往前下方看線 / mount the camera at the front, looking down and forward
# 接線 Wiring：相機 → C 孔；左輪 E、右輪 F / camera → port C; left wheel E, right wheel F
# 方向：站在車後往前看，線在左＝負 / standing behind the robot: line on the left = negative
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from MBC_UniCam import UniCam

hub = PrimeHub()
cam = UniCam(Port.C, mode=41)
left = Motor(Port.E, Direction.COUNTERCLOCKWISE)
right = Motor(Port.F)
robot = DriveBase(left, right, 56, 112)

SPEED = 120       # 👉 可以改：前進速度 mm/s / forward speed
KP = 1.5          # 👉 可以改：偏多少就轉多少 / proportional gain
KD = 4.0          # 👉 可以改：偏移變快時多修一點 / derivative gain
last_error = 0

while True:
    v = cam.line_all()                           # 14 個值一次讀 / all 14 line values at once
    if v is None:                                # 讀不到相機：停車 / no camera: stop
        robot.stop()
        wait(50)
        continue
    near, far, flags, junction, lost = v[0], v[2], v[5], v[6], v[13]
    if junction in (1, 2):                       # 1 十字、2 T 字路口：停車嗶一聲 / cross or T junction: stop
        robot.stop()
        hub.speaker.beep()
        break
    if flags == 0:                               # 近中遠都沒看到線：往最後離線那一邊找 / line lost: search
        robot.drive(0, -120 if lost == 1 else 120)
        wait(10)
        continue
    error = 0.7 * near + 0.3 * far               # 近處為主、遠處提早轉彎 / mostly near, a bit of far
    turn = KP * error + KD * (error - last_error)
    last_error = error
    robot.drive(SPEED, turn)                     # 線在左（負）→ 往左轉 / line on the left → turn left
    wait(10)

# 神攝手 × Pybricks（SPIKE）範例 6：找最大的紅色三角形、轉過去
# SharpShot × Pybricks (SPIKE) example 6: find the biggest red triangle and drive to it
# 需要把相機選單切到 相機模式 5 物體偵測 ・子選項 ① 物體清單＝模式編號 51
# Camera menu: camera mode 5 Object detection, option 1 Object list = mode number 51
# 準備：用要找的物體本身（紅色三角形）教「目標 1」，並指定形狀 Tri（相機選單 Target 1 shape，或 cam.target_shape(1, 3)）
# teach "target 1" with the red triangle itself and set its shape to Tri (camera menu, or cam.target_shape(1, 3))
# 接線 Wiring：相機 → C 孔 / camera → port C；左輪 E、右輪 F / left wheel E, right wheel F
# 同一個資料夾要有 MBC_UniCam.py / MBC_UniCam.py must be imported too
#
# 物體代碼＝色號×10＋形狀：目標 1 的三角形＝1×10＋3＝13。物體 1、2、3、4 是面積由大到小，
# 所以從物體 1 往下找，第一個代碼是 13 的就是「最大的紅色三角形」。
# Object code = colour × 10 + shape: target 1 triangle = 13. Objects 1~4 are sorted largest first,
# so the first object with code 13 is the biggest red triangle.
from pybricks.hubs import PrimeHub
from pybricks.pupdevices import Motor
from pybricks.parameters import Port, Direction, Color
from pybricks.robotics import DriveBase
from pybricks.tools import wait
from MBC_UniCam import UniCam, shape_name

hub = PrimeHub()
cam = UniCam(Port.C, mode=51)                    # 函式庫會自動把相機切到這個模式 / the library switches the camera to this mode
left = Motor(Port.E, Direction.COUNTERCLOCKWISE)
right = Motor(Port.F)
robot = DriveBase(left, right, 56, 112)          # 輪徑 56 mm、輪距 112 mm / wheel 56 mm, axle 112 mm

WANT = 1 * 10 + 3  # 👉 可以改：目標 1 × 10 ＋ 形狀 3（三角）；正方形改成 14、圓改成 11 / square 14, circle 11
KP = 3             # 👉 可以改：偏 1 像素就多轉幾度／秒 / turn rate per pixel of error
NEAR_Y = 100       # 👉 可以改：物體的 Y ≥ 100（畫面最下面）就算到了 / Y at the bottom of the picture = arrived


def find(want):
    # 從最大的物體往下找代碼＝want 的，回傳它的 (X, Y)；找不到 (0, 0) / largest first; (0, 0) = not found
    for i in range(1, cam.max_object() + 1):     # SPIKE 模式 51 給 4 個物體 / 4 objects on SPIKE
        if cam.object(i) == want:
            return cam.object_xy(i)
    return (0, 0)


print('要找 looking for: 目標 target', WANT // 10, shape_name(WANT % 10, True), shape_name(WANT % 10))
while True:
    n = cam.object_count()                       # 畫面裡有幾個物體 0~8 / objects in view
    if n is None:                                # 從來沒讀到相機 / camera never answered
        robot.stop()
        hub.light.on(Color.RED)
        wait(100)
        continue
    hub.display.number(n)
    x, y = find(WANT)
    if x == 0:                                   # 沒看到紅色三角形：原地慢慢轉著找 / not seen: spin to search
        robot.drive(0, 60)
        hub.light.on(Color.ORANGE)
    elif y >= NEAR_Y:                            # 到了：停車、嗶一聲 / arrived: stop and beep
        robot.stop()
        hub.light.on(Color.GREEN)
        hub.speaker.beep()
        break
    else:
        error = x - 80                           # 畫面中間 X = 80；在右 → 正 → 右轉 / right of centre = positive
        robot.drive(80, error * KP)
        hub.light.on(Color.BLUE)
    wait(20)

# 神攝手 × 樂高官方 SPIKE App（教育版 Python）範例 1：跟著色塊轉
# SharpShot × LEGO SPIKE App (Education, Python) example 1: turn to follow a colour blob
# 需要把相機選單切到 相機模式 1 顏色追蹤 ・子選項 ③ 標準（XY＋面積）＝模式編號 13
# Camera menu: camera mode 1 Colour tracking, option 3 Standard (XY + area) = mode number 13
# 準備：先在相機上教「目標 1」的顏色 / teach "target 1" on the camera first
# 接線 Wiring：相機 → C 孔；左輪 E、右輪 F（驅動底座）/ camera → port C; left wheel E, right wheel F
# 用法：SPIKE App 新增「Python」專案，整支貼上 / new Python project in the SPIKE App, paste all of this
# ===== 神攝手 讀值小工具（樂高官方 SPIKE App 3 教育版 Python，照抄就好、不用改）=====
# ===== SharpShot reading helper (LEGO SPIKE App 3, Education, Python: copy as is) =====
# 相機在 SPIKE App 裡是「顏色感應器」：顏色、反射光、原始紅／綠／藍 這 5 個數字（byte 0~7）積木與 Python 都讀得到。
# device.data() 回 8 個值：byte 0、byte 1，之後 6 個 int16（byte 8~13 也讀得到，14~15 讀不到；2026-11-01 上機）。
# 「顏色」改讀 device.data() 的第 1 個值：color_sensor.color() 會把 5 換成 6（樂高的綠色），5 和 6 就分不出來。
# The camera shows up as a colour sensor: colour, reflection, raw red/green/blue (bytes 0~7).
# device.data() gives 8 values: byte 0, byte 1, then 6 int16 (bytes 8~13 readable, 14~15 not; tested 2026-11-01).
# The colour byte is read with device.data(): color_sensor.color() turns 5 into 6 (LEGO green).
# mode = 你在相機選單切的模式編號（相機模式×10＋子選項，例 13）/ mode = camera menu mode number
import color_sensor
import device


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

    def update(self):
        """讀一次最新資料 / read once"""
        c = device.data(self.port)[0]                  # byte 0（color() 會把 5 換成 6 / color() turns 5 into 6）
        r = color_sensor.reflection(self.port)         # byte 1
        rgb = color_sensor.rgbi(self.port)             # 原始紅、綠、藍（各 2 byte）
        b = [int(c) & 255, int(r) & 255]
        for v in rgb[:3]:
            v = int(v) & 0xFFFF                        # 負數也換回 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 cell(self, color):
        """顏色追蹤：色 color 的格子 (X 格, Y 格)，(0, 0)＝沒找到（Y 上＝小）
        模式 13、14：16×12 格（像素 ≈ 格×10−5）；模式 11、12：8×6 格（像素 ≈ 格×20−10），色 1~7"""
        m = self.mode
        if m in (13, 14):
            if color == 1:
                return (self.b[0], self.b[1])
            return (self.b[2 * color - 2], self.b[2 * color - 1])      # 13：色 2~4；14：色 2
        if color == 1:
            g = self.b[1]
        else:
            w = self._w((color - 2) // 2)
            g = w // 100 if color % 2 == 0 else w % 100
        return (g // 10, g % 10)

    def size_cell(self, color):
        """模式 14：色 1、2 的 (寬格, 高格)（16×12 格：寬 ≈ 格×10）"""
        w = self._w(color)                    # 色 1 在原始綠、色 2 在原始藍
        return (w & 255, w >> 8)

    def colors_found(self):
        """模式 11、12：找到幾色（顏色）"""
        return self.b[0]

    def hue(self, point):
        """座標取色：取樣點的色相碼 0~255（21：點 1~6；22、23：點 1~3）"""
        if self.mode == 21:
            return self.b[1 + point]
        return self.b[2 * point]

    def sat(self, point):
        """飽和度（22、23：點 1~3；22 的點 4 在反射光）"""
        if self.mode == 22 and point == 4:
            return self.b[1]
        return self.b[2 * point + 1]

    def val(self, point):
        """明度（23：只有點 1，在反射光；其他點在 byte 8~15）"""
        return self.b[1] if point == 1 else None

    def order(self):
        """照順序給色號：(左起前 6 塊的色相碼, 總塊數)"""
        n = min(6, self.b[0])
        return (self.b[2:2 + n], self.b[0])

    def line(self):
        """循線：(近處偏移, 遠處偏移, 角度, 路口類型, 近處線寬)；偏移、角度 −100~100（線在左＝負）"""
        return (self._w(0) - 100, self._w(1) - 100, self._w(2) - 100, self.b[0], self.b[1])

    # ── 物體偵測（51 物體清單、52 物體詳細）：物體代碼＝色號×10＋形狀，0＝沒有；物體 1＝面積最大 ──
    # ── object detection (51 list, 52 detail): code = colour×10 + shape, 0 = none; object 1 = largest ──
    def _obj(self, i, n, modes=(51, 52)):
        if self.mode not in modes:
            raise ValueError('模式 %d 沒有這個值，要用模式 %s / not available in mode %d, use %s' % (self.mode, modes, self.mode, modes))
        if not 1 <= i <= n:
            raise ValueError('第幾個物體要填 1~%d / object must be 1~%d' % (n, n))

    def max_object(self):
        """byte 0~7 給幾個物體的代碼：51→3、52→2（52 的位置、大小只有物體 1）"""
        self._obj(1, 1)
        return 3 if self.mode == 51 else 2

    def object_count(self):
        """51：畫面裡有幾個物體 0~8（顏色）/ objects in view (colour)"""
        self._obj(1, 1, (51,))
        return self.b[0]

    def object(self, i):
        """物體 i 的代碼（51：物體 1~3；52：物體 1~2）/ object code"""
        if self.mode == 51:
            self._obj(i, 3)
            return self.b[1] if i == 1 else self._w(i - 1) // 100
        self._obj(i, 2)
        return self.b[0] if i == 1 else self._w(2) & 255

    def object_color(self, i):
        """物體 i 的色號 1~8，0＝沒有"""
        return self.object(i) // 10

    def object_shape(self, i):
        """物體 i 的形狀 0~9（沒有物體也是 0）"""
        return self.object(i) % 10

    def object_cell(self, i):
        """51：物體 i（1~3）的 8×6 格 (X 格, Y 格)，(0, 0)＝沒有（Y 上＝小；像素 ≈ 格×20−10）"""
        self._obj(i, 3, (51,))
        g = self._w(0) if i == 1 else self._w(i - 1) % 100
        return (g // 10, g % 10)

    def object_xy(self, i):
        """52：物體 1 的質心 (X, Y) 像素，(0, 0)＝沒有"""
        self._obj(i, 1, (52,))
        return (self._w(0) & 255, self._w(0) >> 8)

    def object_size(self, i):
        """52：物體 1 的外框 (寬, 高) 像素"""
        self._obj(i, 1, (52,))
        return (self._w(1) & 255, self._w(1) >> 8)

    def object_area(self, i):
        """52：物體 1、2 的面積（佔畫面 %）"""
        self._obj(i, 2, (52,))
        return self.b[1] if i == 1 else self._w(2) >> 8


def shape_name(s, zh=False):
    """形狀 0~9 → 'other' 'circle' 'ellipse' 'triangle' 'square' 'rectangle' 'pentagon' 'hexagon' 'star' 'trapezoid'（zh=True 中文）"""
    if s is None or not 0 <= s <= 9:
        return None
    return ('其他', '圓', '橢圓', '三角', '正方', '長方', '五邊', '六邊', '星', '梯形')[s] if zh else         ('other', 'circle', 'ellipse', 'triangle', 'square', 'rectangle', 'pentagon', 'hexagon', 'star', 'trapezoid')[s]


def color_name(h):
    """色相碼 → 'red' 'orange' 'yellow' 'green' 'cyan' 'blue' 'purple' 'white' 'black'"""
    if h > 248:
        return 'black'
    if h > 240:
        return 'white'
    if h < 12 or h >= 220:
        return 'red'
    if h < 28:
        return 'orange'
    if h < 56:
        return 'yellow'
    if h < 100:
        return 'green'
    if h < 140:
        return 'cyan'
    if h < 180:
        return 'blue'
    return 'purple'
# ===== 小工具到這裡結束 / end of helper =====
from hub import port, light_matrix
import motor_pair
import runloop

cam = UniCamApp(port.C, 13)                       # 模式編號要跟相機選單一樣 / must match the camera menu
BASE = 200        # 👉 可以改：前進速度（度／秒）/ forward speed (deg/s)
K = 40            # 👉 可以改：偏 1 格多轉多少 / turn per grid cell of error


async def main():
    motor_pair.pair(motor_pair.PAIR_1, port.E, port.F)
    while True:
        cam.update()
        cx, cy = cam.cell(1)                      # 色 1 的格子：X 1~16、Y 1~12；(0, 0) = 沒看到 / grid cell
        if cx == 0:                               # 沒看到：原地轉著找 / not seen: spin to search
            motor_pair.move_tank(motor_pair.PAIR_1, 120, -120)
            light_matrix.write('?')
        else:
            error = cx - 8.5                      # 畫面中間 = 8.5 格；在右 → 正 / centre = 8.5; right = positive
            motor_pair.move_tank(motor_pair.PAIR_1, int(BASE + K * error), int(BASE - K * error))
            light_matrix.write(str(cx))
        await runloop.sleep_ms(30)

runloop.run(main())

# 神攝手 × 樂高官方 SPIKE App（教育版 Python）範例 2：PD 循線＋路口停車
# SharpShot × LEGO SPIKE App (Education, Python) example 2: PD line following + stop at a junction
# 需要把相機選單切到 相機模式 4 循線 ・子選項 ① 黑線 ＝模式編號 41（白線 42、色線 43 也一樣）
# Camera menu: camera mode 4 Line, option 1 Black line = mode number 41 (42 / 43 work the same)
# 準備：相機裝在車頭、斜斜往前下方看線 / camera at the front, looking down and forward
# 接線 Wiring：相機 → C 孔；左輪 E、右輪 F / camera → port C; left wheel E, right wheel F
# 方向：站在車後往前看，線在左＝負 / standing behind the robot: line on the left = negative
# ===== 神攝手 讀值小工具（樂高官方 SPIKE App 3 教育版 Python，照抄就好、不用改）=====
# ===== SharpShot reading helper (LEGO SPIKE App 3, Education, Python: copy as is) =====
# 相機在 SPIKE App 裡是「顏色感應器」：顏色、反射光、原始紅／綠／藍 這 5 個數字（byte 0~7）積木與 Python 都讀得到。
# device.data() 回 8 個值：byte 0、byte 1，之後 6 個 int16（byte 8~13 也讀得到，14~15 讀不到；2026-11-01 上機）。
# 「顏色」改讀 device.data() 的第 1 個值：color_sensor.color() 會把 5 換成 6（樂高的綠色），5 和 6 就分不出來。
# The camera shows up as a colour sensor: colour, reflection, raw red/green/blue (bytes 0~7).
# device.data() gives 8 values: byte 0, byte 1, then 6 int16 (bytes 8~13 readable, 14~15 not; tested 2026-11-01).
# The colour byte is read with device.data(): color_sensor.color() turns 5 into 6 (LEGO green).
# mode = 你在相機選單切的模式編號（相機模式×10＋子選項，例 13）/ mode = camera menu mode number
import color_sensor
import device


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

    def update(self):
        """讀一次最新資料 / read once"""
        c = device.data(self.port)[0]                  # byte 0（color() 會把 5 換成 6 / color() turns 5 into 6）
        r = color_sensor.reflection(self.port)         # byte 1
        rgb = color_sensor.rgbi(self.port)             # 原始紅、綠、藍（各 2 byte）
        b = [int(c) & 255, int(r) & 255]
        for v in rgb[:3]:
            v = int(v) & 0xFFFF                        # 負數也換回 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 cell(self, color):
        """顏色追蹤：色 color 的格子 (X 格, Y 格)，(0, 0)＝沒找到（Y 上＝小）
        模式 13、14：16×12 格（像素 ≈ 格×10−5）；模式 11、12：8×6 格（像素 ≈ 格×20−10），色 1~7"""
        m = self.mode
        if m in (13, 14):
            if color == 1:
                return (self.b[0], self.b[1])
            return (self.b[2 * color - 2], self.b[2 * color - 1])      # 13：色 2~4；14：色 2
        if color == 1:
            g = self.b[1]
        else:
            w = self._w((color - 2) // 2)
            g = w // 100 if color % 2 == 0 else w % 100
        return (g // 10, g % 10)

    def size_cell(self, color):
        """模式 14：色 1、2 的 (寬格, 高格)（16×12 格：寬 ≈ 格×10）"""
        w = self._w(color)                    # 色 1 在原始綠、色 2 在原始藍
        return (w & 255, w >> 8)

    def colors_found(self):
        """模式 11、12：找到幾色（顏色）"""
        return self.b[0]

    def hue(self, point):
        """座標取色：取樣點的色相碼 0~255（21：點 1~6；22、23：點 1~3）"""
        if self.mode == 21:
            return self.b[1 + point]
        return self.b[2 * point]

    def sat(self, point):
        """飽和度（22、23：點 1~3；22 的點 4 在反射光）"""
        if self.mode == 22 and point == 4:
            return self.b[1]
        return self.b[2 * point + 1]

    def val(self, point):
        """明度（23：只有點 1，在反射光；其他點在 byte 8~15）"""
        return self.b[1] if point == 1 else None

    def order(self):
        """照順序給色號：(左起前 6 塊的色相碼, 總塊數)"""
        n = min(6, self.b[0])
        return (self.b[2:2 + n], self.b[0])

    def line(self):
        """循線：(近處偏移, 遠處偏移, 角度, 路口類型, 近處線寬)；偏移、角度 −100~100（線在左＝負）"""
        return (self._w(0) - 100, self._w(1) - 100, self._w(2) - 100, self.b[0], self.b[1])

    # ── 物體偵測（51 物體清單、52 物體詳細）：物體代碼＝色號×10＋形狀，0＝沒有；物體 1＝面積最大 ──
    # ── object detection (51 list, 52 detail): code = colour×10 + shape, 0 = none; object 1 = largest ──
    def _obj(self, i, n, modes=(51, 52)):
        if self.mode not in modes:
            raise ValueError('模式 %d 沒有這個值，要用模式 %s / not available in mode %d, use %s' % (self.mode, modes, self.mode, modes))
        if not 1 <= i <= n:
            raise ValueError('第幾個物體要填 1~%d / object must be 1~%d' % (n, n))

    def max_object(self):
        """byte 0~7 給幾個物體的代碼：51→3、52→2（52 的位置、大小只有物體 1）"""
        self._obj(1, 1)
        return 3 if self.mode == 51 else 2

    def object_count(self):
        """51：畫面裡有幾個物體 0~8（顏色）/ objects in view (colour)"""
        self._obj(1, 1, (51,))
        return self.b[0]

    def object(self, i):
        """物體 i 的代碼（51：物體 1~3；52：物體 1~2）/ object code"""
        if self.mode == 51:
            self._obj(i, 3)
            return self.b[1] if i == 1 else self._w(i - 1) // 100
        self._obj(i, 2)
        return self.b[0] if i == 1 else self._w(2) & 255

    def object_color(self, i):
        """物體 i 的色號 1~8，0＝沒有"""
        return self.object(i) // 10

    def object_shape(self, i):
        """物體 i 的形狀 0~9（沒有物體也是 0）"""
        return self.object(i) % 10

    def object_cell(self, i):
        """51：物體 i（1~3）的 8×6 格 (X 格, Y 格)，(0, 0)＝沒有（Y 上＝小；像素 ≈ 格×20−10）"""
        self._obj(i, 3, (51,))
        g = self._w(0) if i == 1 else self._w(i - 1) % 100
        return (g // 10, g % 10)

    def object_xy(self, i):
        """52：物體 1 的質心 (X, Y) 像素，(0, 0)＝沒有"""
        self._obj(i, 1, (52,))
        return (self._w(0) & 255, self._w(0) >> 8)

    def object_size(self, i):
        """52：物體 1 的外框 (寬, 高) 像素"""
        self._obj(i, 1, (52,))
        return (self._w(1) & 255, self._w(1) >> 8)

    def object_area(self, i):
        """52：物體 1、2 的面積（佔畫面 %）"""
        self._obj(i, 2, (52,))
        return self.b[1] if i == 1 else self._w(2) >> 8


def shape_name(s, zh=False):
    """形狀 0~9 → 'other' 'circle' 'ellipse' 'triangle' 'square' 'rectangle' 'pentagon' 'hexagon' 'star' 'trapezoid'（zh=True 中文）"""
    if s is None or not 0 <= s <= 9:
        return None
    return ('其他', '圓', '橢圓', '三角', '正方', '長方', '五邊', '六邊', '星', '梯形')[s] if zh else         ('other', 'circle', 'ellipse', 'triangle', 'square', 'rectangle', 'pentagon', 'hexagon', 'star', 'trapezoid')[s]


def color_name(h):
    """色相碼 → 'red' 'orange' 'yellow' 'green' 'cyan' 'blue' 'purple' 'white' 'black'"""
    if h > 248:
        return 'black'
    if h > 240:
        return 'white'
    if h < 12 or h >= 220:
        return 'red'
    if h < 28:
        return 'orange'
    if h < 56:
        return 'yellow'
    if h < 100:
        return 'green'
    if h < 140:
        return 'cyan'
    if h < 180:
        return 'blue'
    return 'purple'
# ===== 小工具到這裡結束 / end of helper =====
from hub import port, light_matrix, sound
import motor_pair
import runloop

cam = UniCamApp(port.C, 41)
BASE = 250        # 👉 可以改：前進速度（度／秒）/ forward speed (deg/s)
KP = 3            # 👉 可以改：比例 / proportional gain
KD = 8            # 👉 可以改：微分 / derivative gain


async def main():
    motor_pair.pair(motor_pair.PAIR_1, port.E, port.F)
    last_error = 0
    while True:
        cam.update()
        near, far, angle, junction, width = cam.line()   # 偏移 −100~100、路口類型、線寬 %
        if junction in (1, 2):                    # 1 十字、2 T 字路口：停車嗶一聲 / cross or T: stop
            motor_pair.stop(motor_pair.PAIR_1)
            await sound.beep(880, 300, 100)
            break
        if width == 0:                            # 近處看不到線：往上一次偏的那邊找 / line lost: search
            turn = -150 if last_error < 0 else 150
            motor_pair.move_tank(motor_pair.PAIR_1, turn, -turn)
        else:
            error = 0.7 * near + 0.3 * far        # 近處為主、遠處提早轉彎 / mostly near, a bit of far
            turn = KP * error + KD * (error - last_error)
            last_error = error
            motor_pair.move_tank(motor_pair.PAIR_1, int(BASE + turn), int(BASE - turn))
        light_matrix.write(str(junction))
        await runloop.sleep_ms(20)

runloop.run(main())