# motion_servo.py  (MicroPython for RP2040)
from machine import Pin, PWM, UART
import time, random
import urandom
from servoclass import Servo
import sys, select, uselect
import math
import animation
import coms
from animation import servos

mode = Pin(20, Pin.IN, Pin.PULL_UP)
LED = Pin(25, Pin.OUT)
yaw_target = 100
yaw_countdown = yaw_target
LED_oscillator = 25
LED_countdown = LED_oscillator
blink_time = 500
blinking = False
speaking_flag = False
speaking_timer = 0
bool_a = False
last_toggle_a= time.ticks_ms()

external = coms.Comms()

# track direction for each servo (1 = going to max, -1 = going to min)
directions = {name: 1 for name in servos.keys()}

last_time = time.ticks_ms()
last_switch = last_time

# animation.servos["YAW"].set_target(90)

on_time = time.ticks_ms()
startup_sleep = True

def facetrack():
    global yaw_countdown, yaw_target

    # ALWAYS read the sensor to prevent serial buffer overflows!
    offset = external.grove_read()

    # 更新 Grove 活跃状态：有数据则标记活跃，超过 3 秒无数据则标记不活跃
    now = time.ticks_ms()
    if offset:
        animation.grove_active = True
        animation.grove_last_seen = now
    elif animation.grove_active and time.ticks_diff(now, animation.grove_last_seen) > 3000:
        animation.grove_active = False

    # Only move the servos if the robot is awake
    if animation.current_state != "idle":
        eyl = animation.servos["EYL"]
        eyr = animation.servos["EYR"]
        pit = animation.servos["PIT"]
        yaw = animation.servos["YAW"]

        if offset:
            dead = external.deadzone
            static = external.staticflag

            x0, y0 = offset
            x_scale = external.x_adj_factor / 110
            y_scale = external.y_adj_factor / 110

            if not static:
                if abs(x0) > dead:
                    x = eyl.target + x0 * x_scale
                    eyl.set_target(x)
                    eyr.set_target(x)

                if abs(y0) > dead:
                    y = pit.target + y0 * y_scale
                    pit.set_target(y)
                    
        if abs(90 - eyl.target) >= 20:
            yaw_countdown -= 1
            if yaw_countdown <= 0:
                yaw.set_target(90 + ((eyl.target-90)/2))
                yaw_countdown = yaw_target
                
# --- STAGGERED STARTUP SEQUENCE ---
time.sleep(0.5) # Allow board power to stabilize after boot

# Determine the correct initial pose based on the calibration pin
is_calibrating = (mode.value() == 1)
initial_pose_dict = animation.pose_calibrate if is_calibrating else animation.pose_sleep

print("Initiating staggered servo wakeup...")
for name, servo_obj in animation.servos.items():
    # Fetch the target angle for this specific pose (default to 90 if missing)
    start_angle = initial_pose_dict.get(name, 90)
    
    # Pre-set the internal tracking positions so the servo math doesn't think it's moving from 90
    servo_obj.pos = start_angle
    servo_obj.target = start_angle
    
    # Send the first pulse. If physically elsewhere, it will snap, but ALONE.
    servo_obj._write_pwm(start_angle)
    
    # Stagger the wakeups. Fast if calibrating, slow and gentle if sleeping.
    delay_ms = 50 if is_calibrating else 250
    time.sleep_ms(delay_ms)

# Set initial states to match the startup logic
animation.current_state = "state_calibrate" if is_calibrating else "idle"
animation.previous_state = animation.current_state
print("Startup complete.")
# ----------------------------------

# external.grove_invoke()

FTDebug = False # setting to True isolates the face tracking code 

while True:
    # Update time 
    now = time.ticks_ms() 
    dt = time.ticks_diff(now, last_time) / 1000.0
    last_time = now
      
    if FTDebug == False:  
        # Grab all pending commands from the ESP
        incoming_commands = external.esp_read()
        for data in incoming_commands:
            if data in animation.action_map:
                # 一次性动作：不改变 current_state
                animation.action_map[data]()
            elif data in animation.state_map:
                animation.new_state_flag = True
                animation.current_state = data
                
        # 2. Check if the state changed this frame
        if animation.current_state != animation.previous_state:
            if animation.current_state == "speaking":
                speaking_flag = True
            elif animation.current_state in ["neutral", "idle", "listening"]:
                if speaking_flag:
                    speaking_flag = False
                    # Snap the mouth closed when speech ends!
                    animation.servos["MOU"].set_target(130)
                    
        # 3. Speaking timer / animation（一次性动作期间暂停，避免覆盖 MOU 目标）
        if speaking_flag and not animation.oneshot_active:
            # Set the mouth position based on the current toggle state
            if bool_a == False:
                animation.servos["MOU"].set_target(130)
            elif bool_a == True:
                animation.servos["MOU"].set_target(70)

            # Flip the boolean every 250ms to flap the mouth
            if time.ticks_diff(now, last_toggle_a) >= 250:
                bool_a = not bool_a   # flip the boolean
                last_toggle_a = now


        # Blink mode enabler
        if not blinking and random.randrange(500) == 0: 
            blinking = True
            blink_counter = blink_time
             

        # 更新一次性动作状态
        animation.update_oneshot()

            # Calibration mode
        if (mode.value() == 1):
            animation.current_state = "state_calibrate"
            animation.apply_state("state_calibrate") # change this back to calibrate to keep calibration mode/base for testing
            LED_countdown -= 1
            if LED_countdown <= 0:
                LED.toggle()
                LED_countdown = LED_oscillator
        elif not animation.oneshot_active:
            # 一次性动作期间跳过常规状态动画，避免舵机目标冲突
            if animation.current_state == "idle":
                animation.apply_state("idle")
                animation.servos["LID"]._write_pwm(30)
            else:
                if animation.previous_state == "idle":
                    animation.servos["LID"]._write_pwm(110)
                    animation.servos["PIT"].set_target(10)
                # Blink mode
                if blinking and animation.current_state != "idle":
                    if blink_counter > blink_time - 50:
                        # closed
                        animation.servos["LID"]._write_pwm(30)
                    elif blink_counter > 0:
                        # reopen
                        animation.servos["LID"]._write_pwm(110)
                    blink_counter -= 1
                    if blink_counter == 0:
                        blinking = False

                # Update with whatever state the ESP says
                animation.apply_state(animation.current_state)                   
                    
        animation.previous_state = animation.current_state
    else:
        animation.servos["LID"]._write_pwm(110)
        animation.current_state = "neutral"
    
    # 一次性动作期间暂停人脸追踪，避免覆盖 EYL/EYR/PIT/YAW 目标
    # 但仍需读取 Grove 数据防止串口缓冲区溢出
    if animation.oneshot_active:
        external.grove_read()  # 消费数据，不驱动舵机
    else:
        facetrack()

    # Update all servos except eyelids
    for name, s in servos.items():
        if name == "LID":
            continue
        s.update(dt)
        



