# ===== 使用前請先看 firmware/stickc/README.md =====
# 1. 在 UIFlow 1.x 打開 Python 畫面，把這整份程式貼上去。
# 2. 改下面「設定」那一段：WiFi、雲端中繼站的網址和密碼。
# 3. 先按 Run 試跑，沒問題再按 Download 存進 StickC。
# ================================================
#
# FlyBrain 仿生六足昆蟲：M5StickC ＋ Hat 8Servos v1.1 ＋ 6 顆 SG-90 伺服馬達。
# 第一階段：用手機打開 Coolify 上的網站，就能從遠端遙控昆蟲。
#
# 訊息怎麼走：
#   手機網頁 -> 雲端網站 -> MQTT 中繼站 -> 這隻 StickC
#   StickC 每 2 秒把自己的狀態（動作、電池、WiFi 訊號）回報給中繼站，網站再轉給手機。
#
# 這份程式改自 FlyBrain 網站 /m5 頁的 main.py：
#   保留：三腳架步態、直接用 I2C 驅動 Hat、慢慢滑回站姿、A 鍵緊急停止。
#   拿掉：Remote+。
#   新增：MQTT 連線、按住連續走、0.8 秒斷線保護、狀態回報、組裝測試。
#
# 接線（Hat 8Servos v1.1 上印的通道編號是 1～8）：
#   左邊三隻腳：L1 前腳、L2 中腳、L3 後腳 -> 接 CH6、CH7、CH8
#   右邊三隻腳：R1 前腳、R2 中腳、R3 後腳 -> 接 CH2、CH3、CH4

from m5stack import *
from m5ui import *
from uiflow import *
from machine import I2C, Pin
import math
import socket
import network
import uselect
import utime
import gc

# ================= 設定（請改成你自己的）=================
WIFI_SSID = ""               # 留空 = 用 UIFlow 裡設定好的 WiFi
WIFI_PASSWORD = ""
MQTT_HOST = "flybrain.example.com"   # 雲端中繼站的網址（Coolify 主機的網域或 IP）
MQTT_PORT = 1883
MQTT_USER = "bug"
MQTT_PASSWORD = "請改成 BUG_MQTT_PASSWORD 的值"
BUG_ID = "bug1"              # 要和雲端的 BUG_ID 一樣
# 觸角接在哪裡："pbhub" = 接在 PbHub（PbHub 再接到 StickC 底部的 Grove 插座）；
#               "gpio"  = 開關直接接 StickC 的 G32（左）、G33（右）
ANT_SOURCE = "pbhub"
PBHUB_LEFT = 1               # 左觸角接 PbHub 的 Port 1
PBHUB_RIGHT = 0              # 右觸角接 PbHub 的 Port 0
PBHUB_PIN = 1                # 開關接在 port 的哪一根訊號線（0 或 1）；實測接的是 IO1
ANT_NC = False               # 觸角開關接的是 NC（常閉）接點就改成 True
OTA_KEY = ""                 # WiFi 更新的密碼；空的 = 不允許 WiFi 更新（USB 寫入工具會自動設定）
# =======================================================
# 用 USB 寫入工具時，上面這些設定會另外存在 /flash/flybrain_cfg.json，開機時讀進來。
# 所以 WiFi 更新只換程式，不會把 WiFi、伺服器位址這些設定洗掉。

FW_VERSION = "1.2"
BUILD = "dev"                # 寫入工具會換成「月日-時分-程式代碼」，螢幕和網頁上看得到
TOPIC_DOWN = "flybrain/" + BUG_ID + "/down"   # 雲端 -> 昆蟲（指令）
TOPIC_UP = "flybrain/" + BUG_ID + "/up"       # 昆蟲 -> 雲端（狀態回報）

# ---------------- wiring 接線 ----------------
LEFT_CHANNELS = [6, 7, 8]                    # 左腳 L1、L2、L3 接的通道
RIGHT_CHANNELS = [2, 3, 4]                   # 右腳 R1、R2、R3 接的通道
# 六隻腳的順序 L1 L2 L3 R1 R2 R3：leg = 0～2 是左腳、3～5 是右腳。
CHANNELS = LEFT_CHANNELS + RIGHT_CHANNELS
# 每隻腳的轉動方向：左右兩邊的馬達是鏡像裝的，所以一邊是 -1、一邊是 +1。
# 組裝測試時如果某隻腳「先往後擺」，就把那隻腳的值正負對調。
LEG_DIRECTION = [-1, -1, -1, 1, 1, 1]
# 每隻腳的微調角度（度）。馬達裝歪了一點，就在這裡補回來。
LEG_TRIM = [0, 0, 0, 0, 0, 0]

# ---------------- 設定檔 ----------------
CFG_FILE = "/flash/flybrain_cfg.json"


def load_cfg():
    global TOPIC_DOWN, TOPIC_UP
    try:
        try:
            import ujson as jsonlib
        except ImportError:
            import json as jsonlib
        f = open(CFG_FILE)
        cfg = jsonlib.load(f)
        f.close()
    except Exception:
        return                       # 沒有設定檔（例如從 UIFlow 貼上）：用上面寫的設定
    g = globals()
    for key in cfg:
        if key in g:
            g[key] = cfg[key]
    TOPIC_DOWN = "flybrain/" + BUG_ID + "/down"
    TOPIC_UP = "flybrain/" + BUG_ID + "/up"


load_cfg()
NEUTRAL = 90             # 站好時每顆馬達的角度
ANGLE_MIN = 10           # 馬達最小只轉到 10 度，避免撞到機構極限
ANGLE_MAX = 170

# ---------------- gait 步態 ----------------
FREQ_HZ = 1.0            # 每秒走幾個步態週期
AMPLITUDE_DEG = 30       # 腳從正中間往前、往後各擺幾度
HARMONIC_RATIO = 0.25    # 讓腳往前收得快、往後推地慢，像真的昆蟲
# 兩組三腳架 {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 毫秒更新一次馬達角度
MOVE_CYCLES = 3          # 點一下前進或後退：走 3 個步態週期
TURN_CYCLES = 1.8        # 點一下左轉、右轉：大約轉 45 度
UTURN_CYCLES = 8         # 點一下迴轉：大約轉 180 度
TEST_CYCLES = 2          # 組裝測試：一隻腳前後擺 2 次

# 按住連續走時，手機每 0.2 秒會送一次「我還按著」。
# 超過 HOLD_TIMEOUT_MS 沒收到，就當作手機放開或斷線了，自己慢慢站好。
HOLD_TIMEOUT_MS = 800
STATE_EVERY_MS = 2000    # 每 2 秒回報一次狀態

# 動作表：名稱 -> (左邊幅度, 右邊幅度, 點一下走幾個週期, 螢幕顯示的文字)。
# 幅度是負的，那一邊的步伐就倒過來（往後走）。兩邊相反，身體就原地轉。
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"),
}
# 可以「按住連續走」的動作。迴轉只能點一下（按住就和右轉一樣，沒有意義）。
HOLD_MOTIONS = ("forward", "backward", "left", "right")


# 找出波形 sin(p) + 0.25 sin(2p) 的最大高度，之後除以它，讓波形最高點剛好是 1。
def _wave_peak():
    largest = 0.0
    for step in range(720):
        p = step * math.pi / 360.0
        height = math.sin(p) + HARMONIC_RATIO * math.sin(2.0 * p)
        if abs(height) > largest:
            largest = abs(height)
    return largest


WAVE_PEAK = _wave_peak()


# 步態波形：輸入相位（弧度），輸出 -1（腳在最後面）到 1（腳在最前面）。
# 公式和 FlyBrain 模擬果蠅的 cpg.py 一樣。
def wave(phase):
    return (math.sin(phase) + HARMONIC_RATIO * math.sin(2.0 * phase)) / WAVE_PEAK


# ---------------- screen 螢幕 ----------------
def use_small_font():
    for font_name in ("FONT_DefaultSmall", "FONT_Small"):
        if hasattr(lcd, font_name):
            lcd.font(getattr(lcd, font_name))
            return


# 螢幕有三行：第一行 FlyBrain，第二行目前動作，第三行連線狀態。
screen_line2 = "BOOT"
screen_line3 = ""


def show(text=None, net=None):
    global screen_line2, screen_line3
    if text is not None:
        screen_line2 = text
    if net is not None:
        screen_line3 = net
    lcd.clear()
    lcd.print("FlyBrain", 4, 6, 0x2dd4bf)
    lcd.print(screen_line2, 4, 24, 0xffffff)
    lcd.print(screen_line3, 4, 42, 0xaaaaaa)
    lcd.print("v" + FW_VERSION + " " + BUILD, 4, 60, 0x777777)


# ---------------- servos 伺服馬達 ----------------
# 不用 UIFlow 內建的 hat.get(hat.SERVOS)：實測 Hat v1.1 插好了它還是報錯，
# 所以自己用 I2C 直接寫 Hat 的暫存器。
HAT_SDA = 0                  # G0：I2C 資料線
HAT_SCL = 26                 # G26：I2C 時脈線
HAT_V11_ADDR = 0x36          # Hat 8Servos v1.1 的 I2C 位址
HAT_V10_ADDR = 0x38          # 舊版 v1.0 的位址
REG_POWER = 0x30             # v1.1 的電源暫存器：寫 1 馬達才有電


def addresses_as_text(addresses):
    hex_addresses = []
    for address in addresses:
        hex_addresses.append(hex(address))
    return str(hex_addresses)


class Servos8Hat:
    def __init__(self):
        self.i2c = I2C(1, sda=Pin(HAT_SDA), scl=Pin(HAT_SCL), freq=100000)
        found = self.i2c.scan()
        if HAT_V11_ADDR in found:
            self.addr = HAT_V11_ADDR
            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))

    # 再打開一次馬達電源。Hat 如果因為電流太大重開機，電源暫存器會變回 0，馬達就沒力了。
    def power_on(self):
        if self.addr == HAT_V11_ADDR:
            self.i2c.writeto_mem(self.addr, REG_POWER, bytes([1]))

    # channel 是 Hat 上印的 1～8；第 n 個通道的角度暫存器在 n - 1。
    def write_servo_angle(self, channel, angle):
        self.i2c.writeto_mem(self.addr, channel - 1, bytes([angle]))


servos = Servos8Hat()
last_angle = [None] * 6      # 每隻腳上一次真正送給馬達的角度
last_pose = [NEUTRAL] * 6    # 每隻腳上一次「想要」的角度（套方向之前）


# 讓第 leg 隻腳轉到 angle 度：套上方向和微調、限制安全範圍，再送給馬達。
def write_leg(leg, angle):
    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
    if angle != last_angle[leg]:
        servos.write_servo_angle(CHANNELS[leg], angle)
        last_angle[leg] = angle


# 六隻腳回到 90 度。一隻一隻來、中間隔 STAND_GAP_MS，
# 避免六顆馬達同一瞬間一起轉，電流太大讓 Hat 斷電。
STAND_GAP_MS = 40


def stand():
    for leg in range(6):
        last_pose[leg] = NEUTRAL
        if last_angle[leg] != NEUTRAL + LEG_TRIM[leg] and leg > 0 and STAND_GAP_MS > 0:
            wait_ms(STAND_GAP_MS)
        write_leg(leg, NEUTRAL)


# 依照步態波形，把六隻腳擺到這個相位該在的位置。
def pose(phase, amp_left, amp_right):
    for leg in range(6):
        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])


# 組裝測試：只有 test_leg 這隻腳前後擺，其他腳站好。
# 用 sin 而不是步態波形，所以腳一定「先往前」擺，方便檢查方向對不對。
def pose_test(phase, test_leg):
    for leg in range(6):
        if leg == test_leg:
            last_pose[leg] = NEUTRAL + AMPLITUDE_DEG * math.sin(phase)
        else:
            last_pose[leg] = NEUTRAL
        write_leg(leg, last_pose[leg])


# ---------------- motion state 動作狀態 ----------------
# 動作結束時，腳可能還在步伐中間。六顆馬達同時跳回 90 度電流會太大，讓 Hat 斷電，
# 所以先用 SETTLE_STEPS 個小步（共 200 毫秒）慢慢滑回站姿。
SETTLE_STEPS = 10

motion = None                # 正在做的動作："forward"…、"test"，None 表示沒在走
holding = False              # True = 按住連續走；False = 點一下走固定步數
test_leg = 0                 # 組裝測試的是哪一隻腳
pending = None               # 腳還沒收回來時又來的動作，格式 (名稱, 是否按住, 測試腳)
steps_left = 0               # 點一下的動作還剩幾毫秒
last_tick_ms = None          # 上一個節拍的時間，用來算真正經過了多久
tick_dt = STEP_MS            # 這一個節拍真正經過的毫秒數
loop_ms = STEP_MS            # 主迴圈一圈平均花多少毫秒（回報給網頁，檢查速度用）
loop_max = 0                 # 上次回報後，最慢的一圈花了幾毫秒
prof = [0, 0, 0, 0]          # 上次回報後各段最慢幾毫秒：收指令、走一步、回報狀態、其他回報
settle_left = 0              # 滑回站姿還剩幾小步
settle_from = [NEUTRAL] * 6
phase = 0.0
last_hold_ms = 0             # 最後一次收到「我還按著」的時間


def now_ms():
    return utime.ticks_ms()


# 走 cycles 個步態週期要幾毫秒。
def cycles_to_steps(cycles):
    return int(round(cycles * 1000 / FREQ_HZ))


def begin(name, hold, leg):
    global motion, holding, test_leg, pending, steps_left, phase
    motion = name
    holding = hold
    test_leg = leg
    pending = None
    phase = 0.0
    if name == "test":
        steps_left = cycles_to_steps(TEST_CYCLES)
    elif name == "auto":
        steps_left = 0
        new_run(brain)
    elif name == "reflex":
        steps_left = 0
        start_rule(reflex_plan)
    else:
        steps_left = cycles_to_steps(MOTIONS[name][2])


def settle():
    global motion, holding, settle_left
    if motion == "auto":
        end_run("stop")
    motion = None
    holding = False
    for leg in range(6):
        settle_from[leg] = last_pose[leg]
    settle_left = SETTLE_STEPS


# 開始一個動作。腳正在走或還沒收回來，就先排隊，等站好再開始，不會突然換動作。
def start(name, hold=False, leg=0):
    global pending
    if name == "test":
        show("TEST " + leg_label(leg))
    elif name == "auto":
        show("AUTO " + brain)
    elif name == "reflex":
        show("REFLEX")
    else:
        show(MOTIONS[name][3])
    if motion is not None:
        pending = (name, hold, leg)
        settle()
    elif settle_left > 0:
        pending = (name, hold, leg)
    else:
        begin(name, hold, leg)


# 手機「我還按著」：同一個動作就只更新時間；不一樣就換動作。
def hold(name):
    global last_hold_ms
    last_hold_ms = now_ms()
    if reflex_busy():
        return                       # 正在躲：先躲完，躲完手機還按著就會接著走
    if motion == name and holding:
        return
    if pending is not None and pending[0] == name and pending[1]:
        return
    start(name, True, 0)


# 手機放開：慢慢站好。
def release():
    global pending
    pending = None
    if motion is not None:
        settle()


# 緊急停止：不滑行，直接站好，清掉排隊中的動作。
# 會重新打開馬達電源、重新送一次所有角度，萬一 Hat 剛剛斷過電也能恢復。
def stop():
    global motion, holding, pending, steps_left, settle_left
    if motion == "auto":
        end_run("estop")
    motion = None
    holding = False
    pending = None
    steps_left = 0
    settle_left = 0
    servos.power_on()
    for leg in range(6):
        last_angle[leg] = None
    stand()
    show("READY")


# A 鍵被按下時 UIFlow 會呼叫這個函式。它可能在主迴圈跑到一半時插進來，
# 所以這裡只「記下來」，真正的停止留給主迴圈做，才不會兩邊同時改狀態、同時寫 I2C。
estop_requested = False
poke_requested = False
next_poke_side = 0


def on_button_a():
    global estop_requested
    estop_requested = True


# B 鍵（側邊）：輪流戳左、右觸角，沒有微動開關時用來測試。
def on_button_b():
    global poke_requested
    poke_requested = True


def leg_label(leg):
    return ("L1", "L2", "L3", "R1", "R2", "R3")[leg]


# 目前狀態，回報給雲端用。
def status_text():
    if motion is not None:
        if holding:
            return motion + "/hold"
        if motion == "auto" and run is not None:
            return "auto/" + run["model"]
        return motion
    if settle_left > 0:
        return "settling"
    return "ready"


# 一個 20 毫秒的節拍。
def tick():
    global phase, steps_left, settle_left, last_tick_ms, tick_dt
    t = now_ms()
    # 步伐照「真正經過的時間」往前推：StickC 算得慢的時候也不會走得比較慢。
    # 上限 60 毫秒：偶爾卡一下（例如重連）時，腳不會一下子跳太遠。
    tick_dt = STEP_MS if last_tick_ms is None else utime.ticks_diff(t, last_tick_ms)
    tick_dt = max(0, min(60, tick_dt))
    last_tick_ms = t
    read_antennas()
    check_reflex()
    if motion == "auto" and run is not None and run_seconds() >= AUTO_MAX_S:
        end_run("time")
        settle()
        return
    if motion is not None:
        # 斷線保護：按住模式下太久沒收到「我還按著」，就自己站好。
        if holding and utime.ticks_diff(now_ms(), last_hold_ms) > HOLD_TIMEOUT_MS:
            settle()
            return
        phase += 2.0 * math.pi * FREQ_HZ * tick_dt / 1000.0
        if motion == "test":
            pose_test(phase, test_leg)
        elif motion == "auto":
            auto_step()
            pose(phase, amps[0], amps[1])
            return
        elif motion == "reflex":
            a = rule_amps()
            if a is None and (antennas[0] or antennas[1]):
                a = keep_turn(antennas)      # 還碰著：繼續轉，直到觸角離開
            if a is None:
                settle()
            else:
                pose(phase, a[0], a[1])
            return
        else:
            pose(phase, MOTIONS[motion][0], MOTIONS[motion][1])
        if not holding:
            steps_left -= tick_dt
            if steps_left <= 0:
                settle()
        return
    if settle_left > 0:
        settle_left -= 1
        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:
            if pending is not None:
                begin(pending[0], pending[1], pending[2])
            else:
                show("READY")


# ---------------- antennas 觸角 ----------------
# 兩個微動開關當觸角：開關的 COM 接 GND、NO 接訊號線。沒碰到讀到 1，碰到讀到 0。
#   PbHub 版：StickC 用 I2C（G32、G33）問 PbHub「Port 幾的訊號線現在是 0 還是 1」。
#   GPIO 版：開關直接接 G32（左）、G33（右），用 StickC 內建上拉電阻。
# 讀不到（PbHub 沒插好）就當作「沒碰到」，不會讓昆蟲亂動。
# 沒有開關也能測試：手機按「戳觸角」，或按 StickC 側邊的 B 鍵（輪流戳左、右）。
ANT_PINS = (32, 33)
ANT_DEBOUNCE_MS = 20     # 訊號要穩定 20 毫秒才算數，避免開關彈跳
POKE_MS = 150            # 戳一下 = 觸角被碰 0.15 秒
PBHUB_ADDR = 0x61        # PbHub 的 I2C 位址
PBHUB_BASE = (0x40, 0x50, 0x60, 0x70, 0x80, 0xA0)   # Port 0～5 的暫存器起點；+4 = 讀 IO0、+5 = 讀 IO1


class PbHub:
    def __init__(self):
        try:
            import i2c_bus                       # UIFlow 管理 Grove 插座（PORTA = G32/G33）的模組
            self.i2c = i2c_bus.get(i2c_bus.PORTA)
        except Exception:
            self.i2c = I2C(0, sda=Pin(32), scl=Pin(33), freq=100000)
        self.ok = PBHUB_ADDR in self.i2c.scan()

    # 讀 PbHub 第 port 個插座的訊號線，回傳 0 或 1。
    def read(self, port):
        self.i2c.writeto(PBHUB_ADDR, bytes([PBHUB_BASE[port] + 4 + PBHUB_PIN]))
        return self.i2c.readfrom(PBHUB_ADDR, 1)[0]


ant_dev = None           # 第一次讀觸角時才準備（PbHub 或兩根 GPIO 腳）
ant_error = False        # 讀觸角出錯（例如 PbHub 沒插好）


def init_antennas():
    global ant_dev
    if ANT_SOURCE == "pbhub":
        try:
            ant_dev = PbHub()
            if not ant_dev.ok:
                show(None, "NO PBHUB")
        except Exception:
            ant_dev = False
            show(None, "NO PBHUB")
    else:
        ant_dev = [Pin(ANT_PINS[0], Pin.IN, Pin.PULL_UP), Pin(ANT_PINS[1], Pin.IN, Pin.PULL_UP)]


# 第 i 個觸角（0 左、1 右）的訊號線電位。讀不到就回傳「沒碰到」的電位。
def ant_level(i):
    global ant_error
    idle = 0 if ANT_NC else 1
    if not ant_dev:
        return idle
    try:
        if ANT_SOURCE == "pbhub":
            level = ant_dev.read(PBHUB_LEFT if i == 0 else PBHUB_RIGHT)
        else:
            level = ant_dev[i].value()
        ant_error = False
        return 1 if level else 0
    except Exception:
        ant_error = True
        return idle


ant_raw = [0, 0]
ant_raw_ms = [0, 0]
ant_switch = [0, 0]
poke_until = [0, 0]
antennas = [0, 0]        # 結果：1 = 碰到
ant_dirty = False        # 觸角狀態改變了，要回報給雲端


def read_antennas():
    global ant_dirty
    if ant_dev is None:
        init_antennas()
    t = now_ms()
    for i in range(2):
        raw = 1 if ant_level(i) == 0 else 0
        if ANT_NC:
            raw = 1 - raw                # 常閉接點：平常是通的，碰到才斷開
        if raw != ant_raw[i]:
            ant_raw[i] = raw
            ant_raw_ms[i] = t
        elif raw != ant_switch[i] and utime.ticks_diff(t, ant_raw_ms[i]) >= ANT_DEBOUNCE_MS:
            ant_switch[i] = raw
        touched = 1 if ant_switch[i] or utime.ticks_diff(poke_until[i], t) > 0 else 0
        if touched != antennas[i]:
            antennas[i] = touched
            ant_dirty = True


def poke(side):
    poke_until[side] = utime.ticks_add(now_ms(), POKE_MS)


# ---------------- brains 避障大腦 ----------------
# 自動模式：昆蟲自己往前走，碰到東西時由「大腦」決定左右兩邊的步伐。
#   neural  神經版：模仿果蠅觸角的機械感覺神經元
#   ifelse  if-else 版：碰左就後退再右轉，碰右就後退再左轉
#   ablated 切除觸角（對照組）：觸角訊號不進大腦，只會直走
BRAINS = ("neural", "ifelse", "ablated")
AUTO_MAX_S = 180         # 自動模式最多跑 3 分鐘，時間到自己站好
AUTO_REPORT_MS = 200     # 每 0.2 秒回報一次大腦的數值
ESCAPE_CLEAR_MS = 1000   # 兩邊觸角都放開 1 秒，才算「脫困」
STUCK_MS = 10000         # 一次碰撞超過 10 秒才脫困，算「卡住」

# 神經版：每個觸角一個神經元，興奮度 v（0～1）。
#   碰到時 v 很快上升，放開後慢慢衰減，所以轉彎是平滑的，而且會持續一下。
#   感覺暫留：觸角被碰一下，神經元會持續收到訊號 NEU_HOLD_MS，碰得再快也感覺得到。
#   左邊興奮 → 往右轉（對側）；興奮度越高越往後退。所以碰到時會「後退帶轉彎 → 原地轉 → 斜走 → 直走」，
#   和手動模式的反射一樣明顯，但是是平滑變化的。兩邊一起興奮 → 後退，再稍微偏右轉。
#   習慣化 h：每碰一次 h 就變大，反應變小（像果蠅對一直碰到的東西越來越不在意），之後慢慢恢復。
NEU_HOLD_MS = 250
NEU_BACK_MS = 400        # 一直碰著超過這麼久，就不再後退，改成原地轉，直到觸角離開
MIN_TURN = 0.7           # 碰著的時候至少要轉這麼快（習慣化了也一樣），不會卡住不動
NEU_RISE_MS = 60
NEU_DECAY_MS = 1000
TURN_GAIN = 1.2
BACK_GAIN = 2.5
BOTH_BIAS = 0.3
HAB_STEP = 0.2
HAB_MAX = 0.8
HAB_DECAY_MS = 15000

# if-else 版：碰到後照固定劇本做，每一步是（左幅度, 右幅度, 毫秒）。
RULE_LEFT = ((-1.0, -1.0, 300), (1.0, -1.0, 900))     # 碰左：後退 0.3 秒、右轉 0.9 秒
RULE_RIGHT = ((-1.0, -1.0, 300), (-1.0, 1.0, 900))    # 碰右：後退 0.3 秒、左轉 0.9 秒
RULE_BOTH = ((-1.0, -1.0, 750), (1.0, -1.0, 1200))    # 兩邊：後退久一點、右轉

brain = "neural"
neu_in = [0, 0]          # 加上感覺暫留後的觸角輸入
neu_seen = [0, 0]        # 最後一次碰到觸角的時間
neu_since = [0, 0]       # 這一次從什麼時候開始碰著
neu_v = [0.0, 0.0]
neu_h = [0.0, 0.0]
prev_ant = [0, 0]
rule_steps = []
rule_until = 0
amps = [1.0, 1.0]
run = None               # 這一次自動模式的統計
reports = []             # 還沒送出的實驗結果（斷線時先存著）


def clamp1(x):
    if x > 1.0:
        return 1.0
    if x < -1.0:
        return -1.0
    return x


def brain_neural(inputs):
    t = now_ms()
    dt = tick_dt
    for i in range(2):
        if inputs[i]:
            neu_seen[i] = t
        felt = 1 if inputs[i] or utime.ticks_diff(t, neu_seen[i]) < NEU_HOLD_MS else 0
        if felt and not neu_in[i]:
            neu_h[i] = neu_h[i] + HAB_STEP * (1.0 - neu_h[i])
            neu_since[i] = t
        neu_in[i] = felt
        neu_h[i] = neu_h[i] - neu_h[i] * dt / HAB_DECAY_MS
        drive = felt * (1.0 - HAB_MAX * neu_h[i])
        tau = NEU_RISE_MS if drive > neu_v[i] else NEU_DECAY_MS
        neu_v[i] = neu_v[i] + (drive - neu_v[i]) * min(1.0, dt / tau)
    vl = neu_v[0]
    vr = neu_v[1]
    turn = TURN_GAIN * (vl - vr) + BOTH_BIAS * min(vl, vr)
    fwd = 1.0 - BACK_GAIN * (vl + vr)
    touching = inputs[0] or inputs[1]
    if touching:
        # 觸角還碰著：一定要轉，轉到離開為止。
        side = 0 if inputs[0] else 1
        if abs(turn) < MIN_TURN:
            turn = MIN_TURN if side == 0 else -MIN_TURN
        held = utime.ticks_diff(t, neu_since[side])
        if held > NEU_BACK_MS and fwd < 0:
            fwd = 0.0                    # 已經退過了，改成原地轉
    return (fwd + turn, fwd - turn)


# 照劇本走：plan 是（左幅度, 右幅度, 毫秒）的清單。
def start_rule(plan):
    global rule_steps, rule_until
    rule_steps = list(plan)
    rule_until = utime.ticks_add(now_ms(), plan[0][2])


# 劇本現在這一步的（左, 右）幅度；劇本走完回傳 None。
def rule_amps():
    global rule_until
    t = now_ms()
    while rule_steps and utime.ticks_diff(t, rule_until) >= 0:
        rule_steps.pop(0)
        if rule_steps:
            rule_until = utime.ticks_add(rule_until, rule_steps[0][2])
    if rule_steps:
        return (rule_steps[0][0], rule_steps[0][1])
    return None


def pick_plan(inputs):
    if inputs[0] and inputs[1]:
        return RULE_BOTH
    if inputs[0]:
        return RULE_LEFT
    return RULE_RIGHT


# 一直碰著時要轉的方向：碰左 → 往右轉，碰右 → 往左轉，兩邊 → 往右轉。
def keep_turn(inputs):
    return pick_plan(inputs)[-1][:2]


def brain_ifelse(inputs):
    # 只有「剛碰到」那一下才開始劇本；一直碰著不會重頭來（不然會一直後退）。
    new_touch = (inputs[0] and not prev_ant[0]) or (inputs[1] and not prev_ant[1])
    if not rule_steps and new_touch:
        start_rule(pick_plan(inputs))
    a = rule_amps()
    if a is None and (inputs[0] or inputs[1]):
        return keep_turn(inputs)         # 劇本做完了還碰著：繼續轉，直到離開
    return a if a is not None else (1.0, 1.0)


# ---------------- reflex 觸角反射（手動模式）----------------
# 手動遙控或站著時碰到觸角：照 if-else 版的劇本躲開（後退、往另一邊轉），躲完站好。
# 自動模式交給大腦處理；組裝測試時不反射。手機可以用「碰到觸角時自動躲開」開關關掉。
reflex_on = True
reflex_plan = RULE_LEFT
man_prev = [0, 0]


def reflex_busy():
    return motion == "reflex" or (pending is not None and pending[0] == "reflex")


def check_reflex():
    global reflex_plan
    edge = (antennas[0] and not man_prev[0]) or (antennas[1] and not man_prev[1])
    man_prev[0] = antennas[0]
    man_prev[1] = antennas[1]
    if not edge or not reflex_on or motion in ("auto", "test") or reflex_busy():
        return
    if pending is not None and pending[0] in ("auto", "test"):
        return
    reflex_plan = pick_plan(antennas)
    start("reflex")


def set_reflex(on):
    global reflex_on
    reflex_on = on


def new_run(model):
    global run, rule_steps
    for i in range(2):
        neu_v[i] = 0.0
        neu_h[i] = 0.0
        neu_in[i] = 0
        prev_ant[i] = antennas[i]
    rule_steps = []
    amps[0] = 1.0
    amps[1] = 1.0
    run = {"model": model, "start": now_ms(), "contacts": 0, "escapes": 0,
           "esc_ms": 0, "stuck": 0, "ep": None, "last": 0}


# 統計：碰撞次數、脫困次數與時間、卡住次數。
def track(inputs):
    t = now_ms()
    for i in range(2):
        if inputs[i] and not prev_ant[i]:
            run["contacts"] += 1
    if inputs[0] or inputs[1]:
        if run["ep"] is None:
            run["ep"] = t
        run["last"] = t
    elif run["ep"] is not None and utime.ticks_diff(t, run["last"]) >= ESCAPE_CLEAR_MS:
        took = utime.ticks_diff(run["last"], run["ep"])
        run["escapes"] += 1
        run["esc_ms"] += took
        if took >= STUCK_MS:
            run["stuck"] += 1
        run["ep"] = None


def auto_step():
    model = brain_of_run()
    if model == "ifelse":
        a = brain_ifelse(antennas)
    elif model == "neural":
        a = brain_neural(antennas)
    else:
        a = brain_neural((0, 0))
    track(antennas)
    prev_ant[0] = antennas[0]
    prev_ant[1] = antennas[1]
    amps[0] = clamp1(a[0])
    amps[1] = clamp1(a[1])


def brain_of_run():
    return run["model"] if run is not None else brain


def run_seconds():
    return utime.ticks_diff(now_ms(), run["start"]) / 1000.0


# 結束這次自動模式，把結果排進要回報的清單。reason：time 時間到、stop 停止、estop 緊急停止。
def end_run(reason):
    global run
    if run is None:
        return
    reports.append("RUN," + run["model"] + "," + str(int(run_seconds() * 10)) + ","
                   + str(run["contacts"]) + "," + str(run["escapes"]) + ","
                   + str(run["esc_ms"]) + "," + str(run["stuck"]) + "," + reason)
    if len(reports) > 20:
        reports.pop(0)
    run = None


def auto_text():
    return ("AUTO," + run["model"] + "," + str(int(run_seconds() * 10)) + ","
            + str(antennas[0]) + "," + str(antennas[1]) + ","
            + str(int(neu_v[0] * 100)) + "," + str(int(neu_v[1] * 100)) + ","
            + str(int(neu_h[0] * 100)) + "," + str(int(neu_h[1] * 100)) + ","
            + str(int(amps[0] * 100)) + "," + str(int(amps[1] * 100)) + ","
            + str(run["contacts"]) + "," + str(run["escapes"]) + ","
            + str(run["esc_ms"]) + "," + str(run["stuck"]))


# ---------------- commands 指令 ----------------
# 雲端送來的指令都是一行文字，用逗號分開：
#   HOLD,forward    按住連續走（每 0.2 秒會再送一次）
#   STEP,left       點一下，走固定步數
#   STOP            放開，慢慢站好
#   ESTOP           緊急站好，馬上回 90 度（組裝時「全部 90 度」也用這個）
#   TEST,3          組裝測試：第 3 隻腳（R1）前後擺
#   PING,12345      測延遲：原封不動回 PONG,12345
#   WHO             網站剛重開、不知道昆蟲是誰時會問：回 HELLO,名字,IP,韌體版本
#   AUTO,neural     自動避障：neural 神經版、ifelse if-else 版、ablated 切除觸角
#   POKE,L          戳左觸角（R 是右觸角），沒有微動開關時測試用
#   REFLEX,1        手動模式碰到觸角自動躲開（REFLEX,0 關掉）
# 回傳：要回給雲端的文字；不用回就回傳 None。
def handle_command(text):
    parts = text.strip().split(",")
    kind = parts[0]
    if kind == "HOLD" and len(parts) == 2 and parts[1] in HOLD_MOTIONS:
        hold(parts[1])
        return None
    if kind == "STEP" and len(parts) == 2 and parts[1] in MOTIONS:
        if not reflex_busy():
            start(parts[1], False, 0)
        return None
    if kind == "STOP":
        release()
        return None
    if kind == "ESTOP":
        stop()
        return None
    if kind == "TEST" and len(parts) == 2 and parts[1] in ("0", "1", "2", "3", "4", "5"):
        start("test", False, int(parts[1]))
        return None
    if kind == "PING" and len(parts) == 2:
        return "PONG," + parts[1]
    if kind == "WHO":
        return hello_text()
    if kind == "AUTO" and len(parts) == 2 and parts[1] in BRAINS:
        set_brain(parts[1])
        start("auto", False, 0)
        return None
    if kind == "POKE" and len(parts) == 2 and parts[1] in ("L", "R"):
        poke(0 if parts[1] == "L" else 1)
        return None
    if kind == "OTA" and len(parts) >= 2:
        return ota_command(parts)
    if kind == "REFLEX" and len(parts) == 2 and parts[1] in ("0", "1"):
        set_reflex(parts[1] == "1")
        return None
    return "ERR,unknown " + text.strip()[:30]


# ---------------- WiFi 更新（OTA）----------------
# 電腦（tools/ota.py）把新的 flybrain.mpy 切成小塊，透過 MQTT 一塊一塊送來：
#   OTA,BEGIN,大小,檢查碼,密碼   開始（只有站好不動時才接受）；檢查碼 = 所有位元組加總除以 65536 的餘數
#   OTA,D,編號,base64資料         一塊資料，StickC 回 OTA,OK,編號 之後電腦才送下一塊
#   OTA,END                       全部送完：檢查大小和檢查碼，對了才換上新程式並重開機
# 舊程式會留成 flybrain_old.mpy；新程式開機失敗時，main.py 會自動換回舊的。
OTA_NEW = "/flash/flybrain_new.mpy"
OTA_CUR = "/flash/flybrain.mpy"
OTA_OLD = "/flash/flybrain_old.mpy"
ota = None               # [檔案, 總大小, 已收到, 檢查碼, 下一塊編號, 應該的檢查碼]
ota_reboot_at = None


def ota_command(parts):
    global ota, ota_reboot_at
    import os
    step = parts[1]
    if step == "BEGIN" and len(parts) == 5:
        if not OTA_KEY or parts[4] != OTA_KEY:
            return "OTA,ERR,key"
        if motion is not None or settle_left > 0:
            return "OTA,ERR,busy"
        if ota is not None:
            ota[0].close()
        ota = [open(OTA_NEW, "wb"), int(parts[2]), 0, 0, 0, int(parts[3])]
        show("UPDATING")
        return "OTA,OK,-1"
    if ota is None:
        return "OTA,ERR,state"
    if step == "D" and len(parts) == 4:
        seq = int(parts[2])
        if seq == ota[4] - 1:
            return "OTA,OK," + parts[2]     # 電腦沒收到回覆又送一次：已經寫過了
        if seq != ota[4]:
            return "OTA,ERR,seq"
        import ubinascii
        data = ubinascii.a2b_base64(parts[3])
        ota[0].write(data)
        ota[2] += len(data)
        ota[3] = (ota[3] + sum(data)) % 65536
        ota[4] += 1
        return "OTA,OK," + parts[2]
    if step == "END":
        f, size, got, check, expect = ota[0], ota[1], ota[2], ota[3], ota[5]
        f.close()
        ota = None
        if got != size or check != expect:
            os.remove(OTA_NEW)
            show("READY")
            return "OTA,ERR,check"
        try:
            os.remove(OTA_OLD)
        except OSError:
            pass
        os.rename(OTA_CUR, OTA_OLD)
        os.rename(OTA_NEW, OTA_CUR)
        show("REBOOT")
        ota_reboot_at = utime.ticks_add(now_ms(), 800)
        return "OTA,DONE"
    return "OTA,ERR,bad"


def set_brain(name):
    global brain
    brain = name


# ---------------- tiny MQTT 迷你 MQTT 用戶端 ----------------
# 自己寫一個很小的 MQTT 3.1.1 用戶端，只做這隻昆蟲用得到的部分：
# 連線、訂閱、發佈（QoS 0）、保持連線。
# 為什麼不用 UIFlow 的 MQTT 積木：它在背景執行緒收訊息，可能和走路的迴圈同時寫 I2C；
# 自己寫就能在同一個迴圈裡「看一眼有沒有訊息」，和原本 UDP 版的做法一樣。
KEEPALIVE_S = 60


def mqtt_string(text):
    data = text.encode()
    return bytes([len(data) >> 8, len(data) & 0xFF]) + data


# MQTT 的「剩餘長度」用 1～4 個位元組表示，每個位元組放 7 個位元。
def mqtt_length(n):
    out = bytearray()
    while True:
        byte = n % 128
        n = n // 128
        if n > 0:
            byte = byte | 0x80
        out.append(byte)
        if n == 0:
            return bytes(out)


def mqtt_packet(header, body):
    return bytes([header]) + mqtt_length(len(body)) + body


def connect_packet(client_id, user, password):
    flags = 0x02                      # clean session
    if user:
        flags = flags | 0x80
    if password:
        flags = flags | 0x40
    body = mqtt_string("MQTT") + bytes([4, flags, KEEPALIVE_S >> 8, KEEPALIVE_S & 0xFF])
    body = body + mqtt_string(client_id)
    if user:
        body = body + mqtt_string(user)
    if password:
        body = body + mqtt_string(password)
    return mqtt_packet(0x10, body)


def subscribe_packet(packet_id, topic):
    body = bytes([packet_id >> 8, packet_id & 0xFF]) + mqtt_string(topic) + bytes([0])
    return mqtt_packet(0x82, body)


def publish_packet(topic, payload):
    return mqtt_packet(0x30, mqtt_string(topic) + payload.encode())


# 從收到的位元組裡切出一個完整的封包。
# 回傳 (封包類型, 內容, 用掉幾個位元組)；資料還不夠一個封包就回傳 None。
def split_packet(buf):
    if len(buf) < 2:
        return None
    length = 0
    multiplier = 1
    pos = 1
    while True:
        if pos >= len(buf):
            return None
        byte = buf[pos]
        length = length + (byte & 0x7F) * multiplier
        multiplier = multiplier * 128
        pos = pos + 1
        if byte & 0x80 == 0:
            break
        if pos > 4:
            raise ValueError("bad MQTT length")
    if len(buf) < pos + length:
        return None
    return (buf[0], bytes(buf[pos:pos + length]), pos + length)


# 解開一個 PUBLISH 封包，回傳 (主題, 內容文字)。
def parse_publish(header, body):
    topic_len = (body[0] << 8) | body[1]
    topic = body[2:2 + topic_len].decode()
    pos = 2 + topic_len
    if (header >> 1) & 0x03:          # QoS 1 或 2 會多兩個位元組的封包編號
        pos = pos + 2
    return (topic, body[pos:].decode())


SEND_TIMEOUT_S = 0.3     # 連上之後，送一個封包最多等 0.3 秒，卡住就當作斷線，不讓腳停在半空中


class TinyMQTT:
    def __init__(self):
        self.sock = None
        self.watch = None
        self.buf = b""
        self.last_ping_ms = 0
        self.last_recv_ms = 0
        self.just_closed = False     # 剛剛斷線了嗎？主迴圈看到後會讓昆蟲站好

    def connected(self):
        return self.sock is not None

    # 連到中繼站並訂閱指令主題。會卡住最多幾秒，所以只在站好時才呼叫。
    def connect(self, host, port, client_id, user, password, topic):
        self.close()
        self.just_closed = False
        addr = socket.getaddrinfo(host, port)[0][-1]
        sock = socket.socket()
        sock.settimeout(5)
        try:
            sock.connect(addr)
            self.sock = sock
            self._send(connect_packet(client_id, user, password))
            packet = self._wait_packet(0x20)
            if len(packet[1]) < 2:
                raise OSError("bad CONNACK")
            if packet[1][1] != 0:
                raise OSError("MQTT refused, code " + str(packet[1][1]))
            self._send(subscribe_packet(1, topic))
            packet = self._wait_packet(0x90)
            # SUBACK 最後一個位元組是 0x80 表示中繼站不准訂閱（通常是帳號權限設錯）。
            if len(packet[1]) < 3 or packet[1][2] == 0x80:
                raise OSError("subscribe refused")
        except Exception:
            # 連線失敗也要把 socket 關掉，不然每 3 秒重試一次，StickC 的 socket 很快就用完了。
            try:
                sock.close()
            except Exception:
                pass
            self.sock = None
            self.buf = b""
            raise
        sock.settimeout(SEND_TIMEOUT_S)
        self.watch = uselect.poll()
        self.watch.register(sock, uselect.POLLIN)
        self.last_recv_ms = now_ms()
        self.last_ping_ms = now_ms()

    def _send(self, data):
        view = data
        while len(view) > 0:
            sent = self.sock.send(view)
            if not sent:
                raise OSError("send failed")
            view = view[sent:]

    # 連線時用：一直等，直到收到指定種類的封包（有 5 秒逾時），中間別的封包丟掉。
    def _wait_packet(self, kind):
        while True:
            packet = split_packet(self.buf)
            if packet is not None:
                self.buf = self.buf[packet[2]:]
                if packet[0] & 0xF0 == kind:
                    return packet
                continue
            data = self.sock.recv(256)
            if not data:
                raise OSError("closed")
            self.buf = self.buf + data

    def publish(self, topic, payload):
        if self.sock is None:
            return
        try:
            self._send(publish_packet(topic, payload))
        except Exception:
            self.close()

    # 看一眼有沒有新訊息，不會卡住。回傳收到的指令文字清單。
    def poll(self):
        messages = []
        if self.sock is None:
            return messages
        try:
            while self.watch.poll(0):
                data = self.sock.recv(256)
                if not data:
                    raise OSError("closed")
                self.buf = self.buf + data
                self.last_recv_ms = now_ms()
            while True:
                packet = split_packet(self.buf)
                if packet is None:
                    break
                self.buf = self.buf[packet[2]:]
                if packet[0] & 0xF0 == 0x30:
                    try:
                        messages.append(parse_publish(packet[0], packet[1])[1])
                    except Exception:
                        pass             # 一則壞掉的訊息就丟掉，不要整條連線斷掉
            # 保持連線：每 30 秒送一個 PINGREQ，中繼站會回 PINGRESP；
            # 90 秒都沒收到任何東西就當作斷線。
            if utime.ticks_diff(now_ms(), self.last_ping_ms) > KEEPALIVE_S * 500:
                self.last_ping_ms = now_ms()
                self._send(bytes([0xC0, 0]))
            if utime.ticks_diff(now_ms(), self.last_recv_ms) > KEEPALIVE_S * 1500:
                raise OSError("timeout")
        except Exception:
            self.close()
        return messages

    def close(self):
        if self.sock is not None:
            self.just_closed = True
            try:
                self.sock.close()
            except Exception:
                pass
        self.sock = None
        self.watch = None
        self.buf = b""


# ---------------- network 網路 ----------------
wlan = network.WLAN(network.STA_IF)
mqtt = TinyMQTT()
last_state_ms = 0
last_state_text = ""
last_auto_ms = 0
next_connect_ms = 0
boot_ms = utime.ticks_ms()


def bug_name():
    mac = wlan.config("mac")
    return "bug-" + "%02x%02x" % (mac[-2], mac[-1])


def hello_text():
    return "HELLO," + bug_name() + "," + wlan.ifconfig()[0] + "," + FW_VERSION + "," + BUILD


def connect_wifi():
    if wlan.isconnected():
        return
    show(None, "WiFi...")
    if WIFI_SSID == "":
        # 用 UIFlow 裡設定好的 WiFi。
        import wifiCfg
        wifiCfg.autoConnect(lcdShow=False)
    else:
        wlan.active(True)
        wlan.connect(WIFI_SSID, WIFI_PASSWORD)
    start_ms = now_ms()
    while not wlan.isconnected() and utime.ticks_diff(now_ms(), start_ms) < 15000:
        wait_ms(200)


# 電池電壓（毫伏）。每 10 秒才真的讀一次，其他時候用上次的值：
# 讀電池很慢的話，走路會每次回報都頓一下。
# StickC Plus2 沒有舊款的電源晶片（axp），問它要等 0.26 秒逾時；Plus2 的電池電壓接在 G38，
# 經過 1/2 分壓，用 ADC 讀。第一次先試 axp：又快又讀得到就用它，不然改用 G38。
BAT_EVERY_MS = 10000
bat_cache = 0
bat_next = 0
bat_mode = None          # "axp" 或 ADC 物件


def battery_mv():
    global bat_cache, bat_next, bat_mode
    t = now_ms()
    if utime.ticks_diff(t, bat_next) < 0:
        return bat_cache
    bat_next = utime.ticks_add(t, BAT_EVERY_MS)
    try:
        if bat_mode is None:
            try:
                v = axp.getBatVoltage()
                if v > 2.5 and utime.ticks_diff(now_ms(), t) < 50:
                    bat_mode = "axp"
            except Exception:
                pass
            if bat_mode is None:
                from machine import ADC
                bat_mode = ADC(Pin(38))
                bat_mode.atten(ADC.ATTN_11DB)
        if bat_mode == "axp":
            bat_cache = int(axp.getBatVoltage() * 1000)
        else:
            raw = bat_mode.read()
            bat_cache = 0 if raw >= 4090 else int(raw * 6600 / 4095)   # 爆表 = 這塊板子不是這樣讀，不顯示
    except Exception:
        bat_cache = 0
    return bat_cache


def wifi_rssi():
    try:
        return wlan.status("rssi")
    except Exception:
        return 0


def send_state(force):
    global last_state_ms, last_state_text, loop_max
    text = status_text()
    due = utime.ticks_diff(now_ms(), last_state_ms) >= STATE_EVERY_MS
    if not force and not due and text == last_state_text:
        return
    uptime_s = utime.ticks_diff(now_ms(), boot_ms) // 1000
    mqtt.publish(TOPIC_UP, "STATE," + text + "," + str(battery_mv()) + ","
                 + str(wifi_rssi()) + "," + str(uptime_s) + "," + ("1" if reflex_on else "0")
                 + "," + str(int(loop_ms)) + "," + str(loop_max)
                 + "," + "/".join([str(x) for x in prof]))
    loop_max = 0
    for i in range(4):
        prof[i] = 0
    last_state_ms = now_ms()
    last_state_text = text


# 沒連上就重連。重連會卡住幾秒，所以只在站好、沒在動的時候做。
def ensure_connected():
    global next_connect_ms
    if mqtt.connected():
        return
    if motion is not None or settle_left > 0:
        return
    if utime.ticks_diff(now_ms(), next_connect_ms) < 0:
        return
    next_connect_ms = utime.ticks_add(now_ms(), 3000)
    try:
        connect_wifi()
        show(None, "MQTT...")
        mqtt.connect(MQTT_HOST, MQTT_PORT, bug_name(), MQTT_USER, MQTT_PASSWORD, TOPIC_DOWN)
        mqtt.publish(TOPIC_UP, hello_text())
        send_state(True)
        show(None, "ONLINE")
    except Exception as error:
        show(None, "NET ERR")
        print("connect failed:", error)


# 主迴圈的一圈（20 毫秒）：收指令、走一步、回報狀態。
def loop_once():
    global estop_requested, poke_requested, next_poke_side, ant_dirty, last_auto_ms
    if ota_reboot_at is not None and utime.ticks_diff(now_ms(), ota_reboot_at) >= 0:
        import machine
        machine.reset()
    if estop_requested:
        estop_requested = False
        stop()
    if poke_requested:
        poke_requested = False
        poke(next_poke_side)
        next_poke_side = 1 - next_poke_side
    t0 = now_ms()
    ensure_connected()
    for text in mqtt.poll():
        reply = handle_command(text)
        if reply is not None:
            mqtt.publish(TOPIC_UP, reply)
    t1 = now_ms()
    tick()
    t2 = now_ms()
    send_state(False)
    t3 = now_ms()
    lap(0, t0, t1)
    lap(1, t1, t2)
    lap(2, t2, t3)
    if mqtt.connected():
        if ant_dirty:
            ant_dirty = False
            mqtt.publish(TOPIC_UP, "ANT," + str(antennas[0]) + "," + str(antennas[1]))
        if (motion == "auto" and run is not None
                and utime.ticks_diff(now_ms(), last_auto_ms) >= AUTO_REPORT_MS):
            last_auto_ms = now_ms()
            mqtt.publish(TOPIC_UP, auto_text())
        while reports and mqtt.connected():
            mqtt.publish(TOPIC_UP, reports.pop(0))
    lap(3, t3, now_ms())
    if mqtt.just_closed:
        # 剛斷線：手動模式就慢慢站好；自動模式繼續避障（大腦在 StickC 上，不需要網路）。
        mqtt.just_closed = False
        if motion != "auto":
            release()
        show(None, "OFFLINE")


def lap(i, a, b):
    d = utime.ticks_diff(b, a)
    if d > prof[i]:
        prof[i] = d


def main():
    global loop_ms, loop_max
    gc.collect()                     # 開始前先整理記憶體，StickC 的記憶體很小
    use_small_font()
    btnA.wasPressed(on_button_a)     # StickC 正面的 A 鍵：緊急停止
    btnB.wasPressed(on_button_b)     # 側邊的 B 鍵：輪流戳左、右觸角
    stop()
    show("READY", "")
    while True:
        began = now_ms()
        try:
            loop_once()
        except Exception as error:
            # 任何意外（例如 Hat 接觸不良、I2C 出錯）都不要讓整個程式停掉：
            # 印出錯誤、試著站好、稍等一下再繼續，遠端的緊急站好也還能用。
            print("loop error:", error)
            try:
                stop()
            except Exception:
                pass
            wait_ms(200)
        spent = utime.ticks_diff(now_ms(), began)
        loop_ms = loop_ms * 0.9 + spent * 0.1
        if spent > loop_max:
            loop_max = spent
        # 一圈固定 20 毫秒：算比較久，就少等一點。
        wait_ms(max(1, STEP_MS - spent))


# 在電腦上做測試或模擬時，會用 "flybrain_firmware" 這個名字載入，就不會自己跑起來。
if __name__ != "flybrain_firmware":
    main()