import network
import espnow
import time
import ubinascii
from machine import I2C, Pin, ADC
from ssd1306 import SSD1306_I2C

# ----------------------------
# 常量定义 - 集中管理配置参数
# ----------------------------
# ADC引脚配置 (连接摇杆)
ADC_X_PIN = 3
ADC_Y_PIN = 2

# 摇杆校准参数 (根据实际硬件调整)
JOYSTICK_CALIB = {
    'x_min': 2175,
    'x_center': 3797,
    'x_max': 4095,
    'y_min': 2175,
    'y_center': 3816,
    'y_max': 4095,
    'threshold': 50  # 死区阈值
}

# OLED屏幕配置
OLED_WIDTH = 128
OLED_HEIGHT = 64
OLED_I2C_SCL = 10
OLED_I2C_SDA = 9

# ESP-NOW配置
TARGET_MAC = b'\xac\x67\xb2\x44\xba\x8c'  # 目标设备MAC地址
LOOP_DELAY = 0.1  # 主循环延迟(秒)

# 方向指令常量
DIRECTIONS = {
    'stop': 'Stop',
    'forward': 'Forward',
    'backward': 'Backward',
    'left': 'Left',
    'right': 'Right'
}

# ----------------------------
# 硬件初始化函数
# ----------------------------
def init_adc():
    """初始化ADC引脚用于读取摇杆值"""
    adc_x = ADC(Pin(ADC_X_PIN))
    adc_y = ADC(Pin(ADC_Y_PIN))
    # 设置衰减以支持0-3.3V范围
    adc_x.atten(ADC.ATTN_11DB)
    adc_y.atten(ADC.ATTN_11DB)
    return adc_x, adc_y

def init_oled():
    """初始化OLED屏幕"""
    i2c = I2C(0, scl=Pin(OLED_I2C_SCL), sda=Pin(OLED_I2C_SDA), freq=400000)
    oled = SSD1306_I2C(OLED_WIDTH, OLED_HEIGHT, i2c)
    oled.fill(0)  # 清屏
    return oled

def init_espnow():
    """初始化ESP-NOW通信"""
    # 初始化WiFi为STA模式
    sta = network.WLAN(network.STA_IF)
    sta.active(True)
    
    # 显示本机MAC地址
    mac_bytes = sta.config('mac')
    mac_str = ubinascii.hexlify(mac_bytes, ':').decode()
    print(f"本机MAC地址: {mac_str}")
    sta.disconnect()  # 不需要连接到AP
    
    # 初始化ESP-NOW
    esp_now = espnow.ESPNow()
    esp_now.active(True)
    
    # 添加目标设备
    esp_now.add_peer(TARGET_MAC)
    target_mac_str = ubinascii.hexlify(TARGET_MAC, ':').decode()
    print(f"已添加目标设备MAC: {target_mac_str}")
    
    return esp_now

# ----------------------------
# 功能函数
# ----------------------------
def calibrate_joystick(times):
    """校准摇杆最小值、中间值、最大值"""
    x = y = 0
    for i in range(times):
        print(adc_x.read(), adc_y.read())
        x = x + adc_x.read()
        y = y + adc_y.read()
        time.sleep(1)
    print(x / 10 , y / 10)

def read_joystick_values(adc_x, adc_y):
    """读取摇杆原始ADC值"""
    return adc_x.read(), adc_y.read()

def calculate_direction(x, y):
    """根据摇杆位置计算方向指令"""
    x_deviation = x - JOYSTICK_CALIB['x_center']
    y_deviation = y - JOYSTICK_CALIB['y_center']
    
    # 判断是否在死区内(停止状态)
    if abs(x_deviation) < JOYSTICK_CALIB['threshold'] and \
       abs(y_deviation) < JOYSTICK_CALIB['threshold']:
        return DIRECTIONS['stop']
    
    # 判断X/Y方向优先级
    if abs(x_deviation) > abs(y_deviation):
        return DIRECTIONS['right'] if x_deviation > 0 else DIRECTIONS['left']
    else:
        return DIRECTIONS['forward'] if y_deviation > 0 else DIRECTIONS['backward']

def calculate_analog_values(x, y):
    """
    计算摇杆的模拟量值(-127~+127)
    返回: (x_analog, y_analog)
    """
    x_deviation = x - JOYSTICK_CALIB['x_center']
    y_deviation = y - JOYSTICK_CALIB['y_center']
    
    # 如果在死区内，返回0
    if abs(x_deviation) < JOYSTICK_CALIB['threshold'] and \
       abs(y_deviation) < JOYSTICK_CALIB['threshold']:
        return 0, 0
    
    # 计算X轴模拟量
    if x_deviation > 0:
        # 右方向：从中心到最大值映射到0~127
        x_range = JOYSTICK_CALIB['x_max'] - JOYSTICK_CALIB['x_center']
        x_analog = int((x_deviation / x_range) * 127)
    else:
        # 左方向：从最小值到中心映射到-127~0
        x_range = JOYSTICK_CALIB['x_center'] - JOYSTICK_CALIB['x_min']
        x_analog = int((x_deviation / x_range) * 127)
    
    # 计算Y轴模拟量
    if y_deviation > 0:
        # 前方向：从中心到最大值映射到0~127
        y_range = JOYSTICK_CALIB['y_max'] - JOYSTICK_CALIB['y_center']
        y_analog = int((y_deviation / y_range) * 127)
    else:
        # 后方向：从最小值到中心映射到-127~0
        y_range = JOYSTICK_CALIB['y_center'] - JOYSTICK_CALIB['y_min']
        y_analog = int((y_deviation / y_range) * 127)
    
    # 限制在-127~127范围内
    x_analog = max(-127, min(127, x_analog))
    y_analog = max(-127, min(127, y_analog))
    
    return x_analog, y_analog

def send_analog_values(esp_now, x_analog, y_analog):
    """
    发送模拟量值到目标设备
    格式: "ANALOG:X:Y" 例如: "ANALOG:100:-50"
    """
    analog_msg = f"ANALOG:{x_analog}:{y_analog}"
    return esp_now.send(TARGET_MAC, analog_msg, True)

def get_centered_position(text, font_width=8, font_height=8):
    """计算文字在OLED屏幕上的居中位置"""
    text_width = len(text) * font_width
    x = (OLED_WIDTH - text_width) // 2
    y = (OLED_HEIGHT - font_height) // 2 + 8  # 垂直居中偏上一点
    return x, y

# ----------------------------
# 主程序
# ----------------------------
def main():
    # 初始化硬件
    adc_x, adc_y = init_adc()
    oled = init_oled()
    esp_now = init_espnow()
    
    # 显示目标设备信息
    target_mac_str = ubinascii.hexlify(TARGET_MAC, ':').decode()
    target_mac_str_no_colon = ubinascii.hexlify(TARGET_MAC).decode()
    oled.text(f"To:{target_mac_str_no_colon}", 0, 0) 
    oled.show()
    
    last_direction = None
    last_x_analog = 0
    last_y_analog = 0
    
    print("控制器启动，开始监控摇杆...")
    
    try:
        while True:
            # 读取并计算方向
            x, y = read_joystick_values(adc_x, adc_y)
            current_direction = calculate_direction(x, y)
            x_analog, y_analog = calculate_analog_values(x, y)          

            # 模拟量模式
            if x_analog != last_x_analog or y_analog != last_y_analog:
                send_result = send_analog_values(esp_now, x_analog, y_analog)
                if send_result:
                    print(f"发送模拟量: X={x_analog:4d}, Y={y_analog:4d}")
                else:
                    print(f"发送失败: X={x_analog:4d}, Y={y_analog:4d}")
                
                # 更新OLED显示
                oled.fill_rect(0, 16, OLED_WIDTH, OLED_HEIGHT-16, 0)
                oled.text(f"X:{x_analog:4d}", 0, 32)
                oled.text(f"Y:{y_analog:4d}", 0, 48)
                oled.show()
                
                last_x_analog = x_analog
                last_y_analog = y_analog

            
            time.sleep(LOOP_DELAY)
            
    except KeyboardInterrupt:
        print("程序被用户终止")
    finally:
        # 清理资源
        oled.fill(0)
        oled.text("Stopped", 40, 32)
        oled.show()
        esp_now.send(TARGET_MAC, DIRECTIONS['stop'], True)
        print("已发送停止指令，程序退出")

if __name__ == "__main__":
    main()