# ===== 使用前請先在 UIFlow 1.x 完成以下步驟 =====
# 1. 切回 Blockly（積木）畫面。
# 2. 打開 Remote+ 設計面板，拉出 5 個 Button，按鈕文字和名稱分別設成：
#    前進(forward)、後退(backward)、左轉(left)、右轉(right)、迴轉(uturn)。
#    Remote+ 網頁上的按鈕是在這個面板建立、存在 M5Stack 雲端的；只貼程式碼不會長出按鈕。
# 3. 切到 Python 畫面填上這份 main.py 的程式碼，
#    再上傳程式：先按 Run 試跑，沒問題再按 Download。
# ================================================

# FlyBrain 迷你六足機器人：M5StickC ＋ Hat 8Servos v1.1 ＋ 6 顆 SG-90 伺服馬達，
# 用手機上的 UIFlow 1.x Remote+ 網頁遙控。
#
# 接線（Hat 8Servos v1.1 上印的通道編號是 1～8）：
#   左邊三隻腳：L1 前腳、L2 中腳、L3 後腳 -> 接 CH6、CH7、CH8
#   右邊三隻腳：R1 前腳、R2 中腳、R3 後腳 -> 接 CH2、CH3、CH4
#
# Remote+ 有五個按鈕：前進、後退、左轉、右轉、迴轉（180 度）。
# 每按一次，機器人就照那個動作走固定的步數，走完自己站好。
# StickC 正面的 A 鍵是緊急停止。
#
# 走路的方式和電腦模擬裡的果蠅一樣（flybrain/cpg.py 的三腳架步態）：
# 六隻腳分成兩組三腳架，A 組 {L1, L3, R2} 和 B 組 {L2, R1, R3} 輪流擺動，
# 所以任何時候都有三隻腳撐在地上，身體不會倒。

# 下面三行載入 UIFlow 的工具：螢幕 lcd、按鍵 btnA、等待 wait_ms、Remote+ 的 remoteInit 等。
from m5stack import *
from m5ui import *
from uiflow import *
# I2C 和 Pin 用來直接和伺服馬達 Hat 溝通（原因寫在「伺服馬達」那一段）。
from machine import I2C, Pin
# math 提供 sin（正弦）和 pi（圓周率），用來算出腳擺動的波形。
import math

# ---------------- wiring 接線 ----------------
# 六隻腳各接在 Hat 的哪一個通道。清單裡的順序就是腳的順序。
LEFT_CHANNELS = [6, 7, 8]                    # 左腳 L1、L2、L3 接的通道
RIGHT_CHANNELS = [2, 3, 4]                   # 右腳 R1、R2、R3 接的通道
# 把兩個清單接起來，得到六隻腳的順序 L1 L2 L3 R1 R2 R3（和 flybrain/config.py 相同）。
# 之後程式裡說「第 leg 隻腳」，leg = 0～2 是左腳、3～5 是右腳。
CHANNELS = LEFT_CHANNELS + RIGHT_CHANNELS
# 每隻腳的轉動方向：+1 表示角度變大時腳往前擺，-1 表示角度變大時腳往後擺。
# 左右兩邊的馬達是鏡像裝的：左邊馬達 0 度是往前、180 度是往後；右邊剛好相反。
LEG_DIRECTION = [-1, -1, -1, 1, 1, 1]
# 每隻腳的微調角度（單位：度）。組裝時馬達裝歪了一點，就在這裡補回來，平常是 0。
LEG_TRIM = [0, 0, 0, 0, 0, 0]
NEUTRAL = 90             # 站好時每顆馬達的角度：90 度是正中間
ANGLE_MIN = 10           # 馬達最小只轉到 10 度，避免撞到機構的極限
ANGLE_MAX = 170          # 馬達最大只轉到 170 度

# ---------------- gait 步態 ----------------
FREQ_HZ = 1.0            # 每秒走幾個步態週期；電池 3.7 V 時 SG-90 跟得上這個速度
AMPLITUDE_DEG = 30       # 腳從正中間往前、往後各擺幾度
# 波形的「歪斜程度」：讓腳往前收得快、往後推地慢，像真的昆蟲一樣。
HARMONIC_RATIO = 0.25
# 每隻腳的起始相位（單位：弧度）。0 和 pi 剛好差半個週期，
# 所以 L1、L3、R2（相位 0）和 L2、R1、R3（相位 pi）兩組三腳架會輪流擺動。
PHASE_OFFSETS = [0.0, math.pi, 0.0, math.pi, 0.0, math.pi]
STEP_MS = 20             # 每 20 毫秒更新一次馬達角度（一秒 50 次）
MOVE_CYCLES = 3          # 按一次前進或後退，走 3 個步態週期
# 轉彎要走幾個週期，是在實體機器人上量出來的：3 個週期大約轉 75 度，6 個週期大約轉 135 度。
# 想重新校正時：新的週期數 = 舊的週期數 × 想要的角度 ÷ 量到的角度。
TURN_CYCLES = 1.8        # 左轉、右轉：3 × 45 ÷ 75 -> 轉 45 度
UTURN_CYCLES = 8         # 迴轉：6 × 180 ÷ 135 -> 轉 180 度

# 動作表：每個按鈕名稱對應一組（左邊幅度, 右邊幅度, 走幾個週期, 螢幕顯示的文字）。
# 幅度 1.0 表示那一邊正常往前走；幅度是負的，那一邊的步伐就倒過來播放（變成往後走）。
# 這是因為波形是「奇函數」：-wave(p) 剛好等於 wave(-p)。
# 轉彎時讓兩邊互相反著走（一邊往前、一邊往後），身體就原地旋轉，
# 比只讓一邊走、另一邊停住轉得快。
MOTIONS = {
    "forward": (1.0, 1.0, MOVE_CYCLES, "FORWARD"),
    "backward": (-1.0, -1.0, MOVE_CYCLES, "BACK"),
    "left": (-1.0, 1.0, TURN_CYCLES, "LEFT"),
    "right": (1.0, -1.0, TURN_CYCLES, "RIGHT"),
    "uturn": (1.0, -1.0, UTURN_CYCLES, "U-TURN"),
}


# 找出波形 sin(p) + HARMONIC_RATIO × sin(2p) 在一個週期裡的最大高度。
# 輸入：沒有。輸出：最大高度（一個正的小數）。
# 為什麼：之後把波形除以這個數，波形的最高點就剛好是 1，擺動角度才會剛好是 AMPLITUDE_DEG。
def _wave_peak():
    largest = 0.0
    # 把一整圈（0 到 2π）切成 720 小格，一格一格算，記下看過的最大值。
    for step in range(720):
        phase_in_radians = step * math.pi / 360.0
        height = math.sin(phase_in_radians) + HARMONIC_RATIO * math.sin(2.0 * phase_in_radians)
        # 波形有正有負，我們要的是離 0 最遠的距離，所以取絕對值。
        if abs(height) > largest:
            largest = abs(height)
    return largest


# 程式一開始就先算好最大高度存起來，之後每次畫波形直接拿來用，不用重算。
WAVE_PEAK = _wave_peak()


# 步態波形：給一個相位（弧度），回傳腳該擺到哪裡，範圍是 -1 到 1。
# 輸入：phase 相位。輸出：-1（腳在最後面）到 1（腳在最前面）之間的數。
# 這一行公式和 FlyBrain 的 flybrain/cpg.py 完全一樣，所以機器人走路的樣子和模擬的果蠅相同。
def wave(phase):
    height = math.sin(phase) + HARMONIC_RATIO * math.sin(2.0 * phase)
    return height / WAVE_PEAK


# ---------------- screen font 螢幕字型 ----------------
# 把螢幕換成小字型。
# 輸入、輸出：都沒有。
# 為什麼：StickC 的螢幕只有 80 像素寬，大字放不下動作名稱。
# 不同版本的 UIFlow 小字型的名字不一樣，所以兩個名字都試，有哪個就用哪個。
def use_small_font():
    for font_name in ("FONT_DefaultSmall", "FONT_Small"):
        # hasattr 檢查 lcd 有沒有這個名字的字型；getattr 用名字把字型拿出來。
        if hasattr(lcd, font_name):
            lcd.font(getattr(lcd, font_name))
            return


# ---------------- servos 伺服馬達 ----------------
# 這支程式不用 UIFlow 內建的 hat.get(hat.SERVOS)，因為實際測試時，
# Hat v1.1 明明插好了它還是報錯「Servos hat maybe not connect」。
# 所以改成自己用 I2C 直接寫 Hat 的暫存器，順便能在螢幕上顯示匯流排上真正找到了什麼。
HAT_SDA = 0                  # StickC 上方排針的 G0 腳：I2C 資料線 SDA
HAT_SCL = 26                 # StickC 上方排針的 G26 腳：I2C 時脈線 SCL
HAT_V11_ADDR = 0x36          # Hat 8Servos v1.1 的 I2C 位址
HAT_V10_ADDR = 0x38          # 舊版 Hat 8Servos v1.0 的位址：角度暫存器一樣，但沒有電源開關
REG_POWER = 0x30             # 只有 v1.1 有的電源暫存器：寫 1 才會打開馬達電源


# 把 I2C 掃到的位址清單轉成好讀的文字，例如 [54, 104] 變成 "['0x36', '0x68']"。
# 輸入：addresses 位址清單（整數）。輸出：一段文字，用來顯示在螢幕和錯誤訊息上。
def addresses_as_text(addresses):
    hex_addresses = []
    for address in addresses:
        hex_addresses.append(hex(address))
    return str(hex_addresses)


# Servos8Hat 代表插在 StickC 上的那塊伺服馬達 Hat。
# 建立它的時候會找出 Hat 是哪一版，之後用 write_servo_angle() 叫某一個通道的馬達轉到某個角度。
class Servos8Hat:
    # 建立時執行一次：打開 I2C、掃描匯流排、認出 Hat 的版本。
    # 找不到 Hat 就在螢幕寫 NO HAT 並停下來，不會假裝在走。
    def __init__(self):
        self.i2c = I2C(1, sda=Pin(HAT_SDA), scl=Pin(HAT_SCL), freq=100000)
        # scan() 問一遍匯流排上所有裝置，回傳有回答的位址清單。
        found = self.i2c.scan()
        if HAT_V11_ADDR in found:
            self.addr = HAT_V11_ADDR
            # v1.1 開機時馬達電源是關的，一定要寫 1 到電源暫存器，馬達才會動。
            self.i2c.writeto_mem(self.addr, REG_POWER, bytes([1]))
        elif HAT_V10_ADDR in found:
            self.addr = HAT_V10_ADDR
        else:
            use_small_font()
            lcd.clear()
            lcd.print("NO HAT", 4, 6, 0xff4444)
            lcd.print("I2C: " + addresses_as_text(found), 4, 24, 0xffffff)
            raise OSError("8Servos hat not on I2C, found " + addresses_as_text(found))

    # 叫一顆馬達轉到指定角度。
    # 輸入：channel 是 Hat 上印的通道 1～8；angle 是 0～180 的整數角度。
    # Hat 的第 n 個通道的角度暫存器位址是 n - 1（CH1 在 0x00、CH2 在 0x01……）。
    def write_servo_angle(self, channel, angle):
        self.i2c.writeto_mem(self.addr, channel - 1, bytes([angle]))


servos = Servos8Hat()
# 每隻腳上一次真正送給馬達的角度。一開始還沒送過，所以是 None（沒有值）。
last_angle = [None] * 6
# 每隻腳上一次「想要」的角度（還沒套上方向和微調之前），滑回站姿時要從這裡出發。
last_pose = [NEUTRAL] * 6


# 讓第 leg 隻腳轉到 angle 度。
# 輸入：leg 是 0～5 的腳編號；angle 是「想要」的角度（90 = 正中間）。
# 這裡會先套上這隻腳的方向和微調，再把角度限制在安全範圍裡，最後才送給馬達。
def write_leg(leg, angle):
    # 套方向：離正中間多少度，乘上 +1 或 -1，左右兩邊的鏡像就對上了；再加上微調。
    angle = NEUTRAL + LEG_DIRECTION[leg] * (angle - NEUTRAL) + LEG_TRIM[leg]
    # 馬達只接受整數角度，先四捨五入。
    angle = int(round(angle))
    # 限制範圍：太小就用最小值，太大就用最大值。
    if angle < ANGLE_MIN:
        angle = ANGLE_MIN
    if angle > ANGLE_MAX:
        angle = ANGLE_MAX
    # 角度和上次一樣就不送，讓 I2C 匯流排安靜一點。
    if angle != last_angle[leg]:
        servos.write_servo_angle(CHANNELS[leg], angle)
        last_angle[leg] = angle


# 六隻腳全部回到正中間（站姿）。
def stand():
    for leg in range(6):
        last_pose[leg] = NEUTRAL
        write_leg(leg, NEUTRAL)


# 依照步態波形，把六隻腳擺到這個相位該在的位置。
# 輸入：phase 目前的相位；amp_left、amp_right 是左右兩邊的幅度（來自動作表 MOTIONS）。
def pose(phase, amp_left, amp_right):
    for leg in range(6):
        # leg 0～2 是左腳，用左邊的幅度；3～5 是右腳，用右邊的幅度。
        if leg < 3:
            amplitude = AMPLITUDE_DEG * amp_left
        else:
            amplitude = AMPLITUDE_DEG * amp_right
        # 每隻腳加上自己的相位差，兩組三腳架才會輪流擺。
        last_pose[leg] = NEUTRAL + amplitude * wave(phase + PHASE_OFFSETS[leg])
        write_leg(leg, last_pose[leg])


# ---------------- screen 螢幕 ----------------
# 清掉螢幕，上面一行寫 FlyBrain，下面一行寫 text（例如目前的動作名稱）。
def show(text):
    lcd.clear()
    lcd.print("FlyBrain", 4, 6, 0x2dd4bf)
    lcd.print(text, 4, 24, 0xffffff)


# ---------------- motion state 動作狀態 ----------------
# 轉彎走 1.8 個週期，停下來時腳還在步伐中間，每隻腳偏了大約 30 度。
# 如果六顆馬達同一瞬間一起跳回 90 度，電流會突然變很大，讓 Hat 斷電。
# 所以動作結束（或被別的按鈕打斷）時，先用 SETTLE_STEPS 個小步慢慢滑回站姿。
# 每個動作也都從站姿、相位 0 開始，所以同一個按鈕每次轉的角度都一樣。
SETTLE_STEPS = 10            # 滑回站姿分 10 小步，共 200 毫秒，每步大約 3 度

motion = None                # 正在做的動作名稱；None 表示沒有在走
pending = None               # 腳還沒收回來時又按的按鈕，等站好再做
steps_left = 0               # 這個動作還剩幾個 20 毫秒小步
settle_left = 0              # 滑回站姿還剩幾小步
settle_from = [NEUTRAL] * 6  # 開始滑回時每隻腳的位置
phase = 0.0                  # 目前的步態相位（弧度）


# 開始做一個動作：從相位 0 開始，算出這個動作要走幾個 20 毫秒小步。
# 輸入：name 動作名稱，例如 "forward"。
# 例如前進：3 個週期 × 1000 毫秒 ÷ (每秒 1 個週期 × 每步 20 毫秒) = 150 小步。
def begin(name):
    # global 表示要改的是函式外面那個變數，不是在函式裡另外建一個新的。
    global motion, pending, steps_left, phase
    motion = name
    pending = None
    phase = 0.0
    cycles = MOTIONS[name][2]
    steps_left = int(round(cycles * 1000 / (FREQ_HZ * STEP_MS)))


# 停止步態，從腳現在的位置開始慢慢滑回站姿。
def settle():
    global motion, settle_left
    motion = None
    for leg in range(6):
        settle_from[leg] = last_pose[leg]
    settle_left = SETTLE_STEPS


# Remote+ 按鈕會呼叫這個函式。
# 輸入：name 動作名稱。
# 如果腳正在走或還沒收回來，就先記下這個按鈕，等腳回到站姿再開始，不會突然換動作。
def start(name):
    global pending
    show(MOTIONS[name][3])
    if motion is not None:
        # 正在走：記下新動作，先滑回站姿。
        pending = name
        settle()
    elif settle_left > 0:
        # 正在滑回站姿：記下新動作，滑完就開始。
        pending = name
    else:
        # 已經站好：直接開始。
        begin(name)


# A 鍵的緊急停止：不等待、不滑行，直接站好，並清掉所有排隊中的動作。
def stop():
    global motion, pending, steps_left, settle_left
    motion = None
    pending = None
    steps_left = 0
    settle_left = 0
    stand()
    show("READY")


# 一個 20 毫秒的節拍：如果正在走，就讓步態往前一小步；如果正在滑回站姿，就滑一小步。
# 主迴圈（積木版是 Loop 積木）每次呼叫這個函式，然後等 STEP_MS 毫秒。
def tick():
    global phase, steps_left, settle_left
    if motion is not None:
        amp_left = MOTIONS[motion][0]
        amp_right = MOTIONS[motion][1]
        # 相位每 20 毫秒前進一點：一秒走 FREQ_HZ 圈，一圈是 2π。
        phase += 2.0 * math.pi * FREQ_HZ * STEP_MS / 1000.0
        pose(phase, amp_left, amp_right)
        steps_left -= 1
        if steps_left <= 0:
            settle()
        return
    if settle_left > 0:
        settle_left -= 1
        # share 從 0.9 慢慢變到 0：腳離正中間的距離每一步縮小一點。
        share = settle_left / SETTLE_STEPS
        for leg in range(6):
            last_pose[leg] = NEUTRAL + (settle_from[leg] - NEUTRAL) * share
            write_leg(leg, last_pose[leg])
        if settle_left == 0:
            # 滑完了：有排隊的動作就開始做，沒有就顯示 READY。
            if pending is not None:
                begin(pending)
            else:
                show("READY")


# ---------------- Remote+ buttons 手機按鈕 ----------------
# 在 UIFlow 的 Remote+ 頁面加五個 Button，每個按鈕各有自己的 callback（被按下時執行的函式）。
# 如果 UIFlow 幫按鈕取了不同的函式名稱，保留 UIFlow 的名稱，把 start(...) 那一行搬進去就好。
def button_forward_callback():
    start("forward")


def button_backward_callback():
    start("backward")


def button_left_callback():
    start("left")


def button_right_callback():
    start("right")


def button_uturn_callback():
    start("uturn")


# ---------------- 開機後執行 ----------------
use_small_font()
btnA.wasPressed(stop)        # StickC 正面的大按鈕（A 鍵）：緊急停止
remoteInit()                 # 啟動 Remote+ 連線，手機按鈕才會有反應
stop()                       # 一開機先站好，螢幕顯示 READY

# 主迴圈：永遠重複。每 20 毫秒做一個節拍。
# 迴圈裡沒有長時間的等待，所以按鈕隨時都按得進來。
while True:
    tick()
    wait_ms(STEP_MS)