#!/usr/bin/env python
# -*- coding: utf-8 -*-

from pico_i2c_lcd import I2cLcd
from machine import Pin, I2C,PWM
import time
import json,uasyncio

config={
    "PWM":{
        "VALUE":65535,"MIN":5,"MAX":65535},#PWM 当前值 最小值 最大值
    "ACCEL":{
        "VALUE":250,"MIN":100,"MAX":6000},#加速度（毫秒） 当前值 最小值 最大值
    "DECEL":{
        "VALUE":250,"MIN":100,"MAX":6000},#加速度（毫秒） 当前值 最小值 最大值
    "SLICES":{
        "VALUE":100,"MIN":50,"MAX":10000}, #平滑度(切片数) 当前值 最小值 最大值
    "FREQ":{
        "VALUE":18000,"MIN":10000,"MAX":60000}, #PWM频率 当前值 最小值 最大值
    "DIR0":{
        "VALUE":True}, #旋转方向 True正转 False反转
    "DIR1":{
        "VALUE":False}, #旋转方向 True正转 False反转
    "DIR2":{
        "VALUE":True}, #旋转方向 True正转 False反转
    "DIR3":{
        "VALUE":False}, #旋转方向 True正转 False反转
    "FIXED0":{
        "VALUE":3000,"MIN":200,"MAX":60000}, #运行时间（毫秒） 当前值 最小值 最大值
    "FIXED1":{
        "VALUE":3000,"MIN":200,"MAX":60000}, #运行时间（毫秒） 当前值 最小值 最大值
    "FIXED2":{
        "VALUE":3000,"MIN":200,"MAX":60000}, #运行时间（毫秒） 当前值 最小值 最大值
    "FIXED3":{
        "VALUE":3000,"MIN":200,"MAX":60000}, #运行时间（毫秒） 当前值 最小值 最大值

}
i2c = None
lcd = None

def lcdinit():
    global i2c,lcd
    i2c = I2C(0, scl=Pin(21), sda=Pin(20))
    lcd = I2cLcd(i2c, 0x27, 2, 16) 

def main():
    pwminit()
    lcdinit()
    global config
    lc=load_config()
    if lc!=None:
        config=lc
    else:
        save_config()
    lcdupdate()
    uasyncio.create_task(licensebtn())
    while True:
        await uasyncio.sleep_ms(100)

'''
按钮事件
在待机界面->
    EN长按进入参数选择界面
    ADD,忽略
    SUB,忽略
在参数选择界面->
    EN短按,进入参数编辑界面
    EN长按，返回待机界面
    ADD,下一条
    SUB,上一条
在参数编辑界面
    EN长按,保存返回参数选择界面
    EN短按,切换参数倍率
    ADD,增加
    SUB,减少
'''
SATAE=0 #状态 0待机 1参数选择 2参数设置
CURSOR=0    #焦点
ARGFLOD=1   #倍率

# 定义按钮引脚
buttons = {
    10: Pin(10, Pin.IN, Pin.PULL_UP),
    11: Pin(11, Pin.IN, Pin.PULL_UP),
    12: Pin(12, Pin.IN, Pin.PULL_UP),
    13: Pin(13, Pin.IN, Pin.PULL_UP),
    14: Pin(14, Pin.IN, Pin.PULL_UP)
}

# 长按和短按的时间阈值（单位：秒）
LONG_PRESS_THRESHOLD = 2.0  # 长按定义为1秒及以上
DEBOUNCE_DELAY = 0.02       # 防抖延时，单位：秒


async def licensebtn(): #监听按钮
    tasks = [monitor_button(pin_number) for pin_number in buttons]
    await uasyncio.gather(*tasks)

# 定义处理方法
async def handle_short_press(pin_number):
    print(f"按钮 {pin_number} 短按")
    if pin_number==10:
        handle_button_EN()
    elif pin_number==11:
        handle_button_ADD()
    elif pin_number==12:
        handle_button_SUB()
    elif pin_number==13:
        pass
    elif pin_number==14:
        handle_button_START()

async def handle_long_press(pin_number):
    print(f"按钮 {pin_number} 长按")
    if pin_number==10 :#EN长按
        handle_button_EN_long()
        

async def monitor_button(pin_number):
    """检测单个按钮的状态，区分长按和短按"""
    pin = buttons[pin_number]
    last_state = pin.value()  # 初始状态
    press_start_time = None  # 记录按下的开始时间
    
    while True:
        current_state = pin.value()
        if current_state != last_state:  # 检测到状态变化
            await uasyncio.sleep(DEBOUNCE_DELAY)  # 固定防抖延时
            if pin.value() == current_state:  # 确认状态稳定
                if current_state == 0:  # 按下状态
                    press_start_time = time.time()  # 记录按下开始时间
                else:  # 松开状态
                    if press_start_time is not None:
                        press_duration = time.time() - press_start_time
                        if press_duration >= LONG_PRESS_THRESHOLD:  # 长按
                            await handle_long_press(pin_number)
                        else:  # 短按
                            await handle_short_press(pin_number)
                        press_start_time = None  # 重置开始时间
                last_state = current_state  # 更新状态
        await uasyncio.sleep(0.01)  # 轮询延时，避免占用过多 CPU


def lcdupdate():
    if SATAE==0:#待机
        print('待机界面')
        lcd.clear()
        lcd.move_to(0,0)
        lcd.putstr('P:'+str(config['PWM']['VALUE']))
        lcd.move_to(8,0)
        lcd.putstr('F:'+str(config['FIXED0']['VALUE']))
        lcd.move_to(0,1)
        lcd.putstr('A:'+str(config['ACCEL']['VALUE']))
        lcd.move_to(8,1)
        lcd.putstr('D:'+str(config['DECEL']['VALUE']))
    elif SATAE==1:#参数选择
        print('参数选择')
        keys=list(config.keys())
        #print(keys) 
        lcd.clear()
        lcd.move_to(0,0)
        key=keys[CURSOR]
        lcd.putstr(key)
        lcd.move_to(0,1)
        lcd.putstr(str(config[key]["VALUE"]))
    elif SATAE==2:#参数设置
        print('参数设置')
        lcd.clear()
        keys=list(config.keys())
        #print(keys) 
        lcd.clear()
        lcd.move_to(0,0)
        key=keys[CURSOR]
        lcd.putstr(key)
        lcd.move_to(0,1)
        lcd.putstr(str(config[key]["VALUE"]))
        lcd.move_to(9,1)
        lcd.putstr(f"*{ARGFLOD}")
    elif SATAE==255:#机器运行中
        print('机器运行中')
        lcd.clear()
        lcd.putstr("RUNNING...")

def handle_button_EN_long():#GP10长按
    global SATAE
    if SATAE==0:#待机
        #EN长按,进入参数选择
        SATAE=1
        lcdupdate()
    elif SATAE==1:#参数选择
        #EN长按,返回主菜单并保存
        SATAE=0
        lcdupdate()
        save_config()
    elif SATAE==2:#参数编辑
        #EN长按,返回参数选择
        SATAE=1
        lcdupdate()
    print(SATAE)

def handle_button_EN():#GP10
    global ARGFLOD,SATAE
    if SATAE==0:#待机界面
        pass
    elif SATAE==1:#参数选择
        SATAE=2
        lcdupdate()
    elif SATAE==2:#编辑状态
        #EN短按,切换参数倍率
        if ARGFLOD==1:
            ARGFLOD=10
        elif ARGFLOD==10:
            ARGFLOD=100
        elif ARGFLOD==100:
            ARGFLOD=1000
        elif ARGFLOD==1000:
            ARGFLOD=1
        print(ARGFLOD)
        lcdupdate()

def handle_button_ADD():#GP11
    global CURSOR
    if SATAE==0:#待机界面
        pass
    elif SATAE==1:#参数选择
        if CURSOR<len(config)-1:
            CURSOR=CURSOR+1
            lcdupdate()
            print(CURSOR)
    elif SATAE==2:#参数编辑
        keys=list(config.keys())
        key=keys[CURSOR]
        VALUE=config[key]["VALUE"]
        if type(VALUE)==int:
            MAX=config[key]["MAX"]
            config[key]["VALUE"]+=ARGFLOD
            if VALUE>MAX:
                config[key]["VALUE"]=MAX
        elif type(VALUE)==bool:
            config[key]["VALUE"]=not config[key]["VALUE"]
        lcdupdate()

def handle_button_SUB():#GP12
    global CURSOR
    if SATAE==0:#待机界面
        pass
    elif SATAE==1:#参数选择
        if CURSOR>0:
            CURSOR=CURSOR-1
            lcdupdate()
            print(CURSOR)
    elif SATAE==2:#参数编辑
        keys=list(config.keys())
        key=keys[CURSOR]
        VALUE=config[key]["VALUE"]
        if type(VALUE)==int:
            MIN=config[key]["MIN"]
            config[key]["VALUE"]-=ARGFLOD
            if VALUE<MIN:
                config[key]["VALUE"]=MIN
        elif type(VALUE)==bool:
            config[key]["VALUE"]=not config[key]["VALUE"]
        lcdupdate()

def handle_button_START():#GP14
    global SATAE
    if SATAE!=255:
        SATAE=255
        lcdupdate()
        uasyncio.create_task(motor_control())
    else:
        SATAE=0
        lcdupdate()


def handle_button_ESTOP():#GP13急停
    global SATAE
    print('急停')
    SATAE=0
    lcdupdate()
    for p in pwm:
        p.duty_u16(stop_duty)  # **确保电机完全停止（反向逻辑）**

# 定义方向引脚
dirs = {
    2: Pin(2, Pin.OUT),
    4: Pin(4, Pin.OUT),
    6: Pin(6, Pin.OUT),
    8: Pin(8, Pin.OUT),
}

def updatedirs(): 
    dir_keys = ["DIR0", "DIR1", "DIR2", "DIR3"]  # 配置中的方向键
    dir_pins = [2, 4, 6, 8]  # 对应的电机方向引脚

    for i in range(4):
        value = config[dir_keys[i]]["VALUE"]
        if value not in [True, False]:  # **检查值是否为布尔值**
            print(f"无效值：{value}，请填写 True 或 False")
            return
        
        dirs[dir_pins[i]].value(value)  # **更新引脚值**
        print(f"IO口 {dir_pins[i]} 设置为 {'正转' if value else '反转'}")

# 保存到文件
def save_config(filename="config.json"):
    with open(filename, "w") as file:
        json.dump(config, file)
    print(f"配置已保存到 {filename}")
    updatedirs()


# 从文件加载配置
def load_config(filename="config.json"):
    try:
        with open(filename, "r") as file:
            config = json.load(file)
        print(f"配置已从 {filename} 加载")
        updatedirs()
        return config
    except Exception as e:
        print(f"加载配置失败: {e}")
        return None


# 初始化 PWM
pwm_pin = [machine.Pin(pin) for pin in [3, 5, 7, 9]]  # 使用列表推导式初始化引脚
stop_duty = config["PWM"]["MAX"]  # 最大功率仍是 65535
pwm = []  # 确保 `pwm` 作为全局变量初始化

def pwminit():
    global pwm  # 显式声明 pwm 为全局变量
    FREQ = config['FREQ']['VALUE']
    pwm = [PWM(pin) for pin in pwm_pin]  # 重新赋值 pwm
    for p in pwm:
        p.freq(FREQ)          # 设置 PWM 频率
        p.duty_u16(stop_duty) # 使电机完全停止

# **设定参数（用户自定义加速和减速时间）**
total_time_ms = [0] * 4
accel_time_ms = [0] * 4
decel_time_ms = [0] * 4
cruise_time_ms = [0] * 4
slices = 0

# **生成加速度和减速曲线（反向计算）**
curve_accel = [[] for _ in range(4)]
curve_decel = [[] for _ in range(4)]

def updatePWMparam():
    global total_time_ms, accel_time_ms, decel_time_ms, cruise_time_ms, slices
    slices = config['SLICES']['VALUE']  # **时间切片数**
    
    for i in range(4):  # **分别更新每个电机的参数**
        total_time_ms[i] = config[f'FIXED{i}']['VALUE']  # **总运行时间**
        accel_time_ms[i] = config['ACCEL']['VALUE']      # **加速时间**
        decel_time_ms[i] = config['DECEL']['VALUE']      # **减速时间**
        cruise_time_ms[i] = total_time_ms[i] - accel_time_ms[i] - decel_time_ms[i]  # **动态计算匀速时间**
    
def calccurve():
    global curve_accel, curve_decel
    updatePWMparam()
    max_duty = config["PWM"]["MAX"]  # 65535
    target_duty = max_duty-config["PWM"]["VALUE"]  # 设定值

    for i in range(4):
        curve_accel[i] = []
        curve_decel[i] = []

        for j in range(slices + 1):  # **加速：从 MAX 逐步降低到 VALUE**
            duty_value = max_duty - int((j / slices) ** 2 * (max_duty - target_duty))
            curve_accel[i].append(duty_value)

        for j in range(slices + 1):  # **减速：从 VALUE 逐步增加到 MAX**
            duty_value = target_duty + int((j / slices) ** 2 * (max_duty - target_duty))
            curve_decel[i].append(duty_value)

    print("加速曲线:", 
    )
    print("减速曲线:", curve_decel)

# **电机加速**
async def motor_accelerate():
    step_times = [accel_time_ms[i] // slices for i in range(4)]  # 每个电机的加速步长
    for j in range(slices + 1):  # 统一循环控制每个电机
        if SATAE == 255:
            for i in range(4):  # **同时控制所有电机**
                pwm[i].duty_u16(curve_accel[i][j])  # 反向 PWM
            await uasyncio.sleep_ms(min(step_times))  # 采用最小步长来同步执行
        else:
            break

# **电机减速**
async def motor_decelerate(motor_index=None):
    if motor_index is None:  # 如果没有指定电机，控制所有电机减速
        step_times = [decel_time_ms[i] // slices for i in range(4)]  # 每个电机的减速步长
        for j in range(slices + 1):
            if SATAE == 255:
                for i in range(4):  # **分别控制每个电机**
                    pwm[i].duty_u16(curve_decel[i][j])  # 反向 PWM
                await uasyncio.sleep_ms(min(step_times))  # 采用最小步长来同步执行
            else:
                break
    else:  # **只控制单个电机减速**
        step_time = decel_time_ms[motor_index] // slices
        for j in range(slices + 1):
            if SATAE == 255:
                pwm[motor_index].duty_u16(curve_decel[motor_index][j])  # 反向 PWM
                await uasyncio.sleep_ms(step_time)
            else:
                break


async def motor_control():
    global SATAE
    calccurve()
    print(f"电机启动（总时间 {total_time_ms} ms），加速 {accel_time_ms} ms...")
    
    # **所有电机同时启动加速**
    print(">>> 开始电机加速")
    start_accel = time.ticks_ms()
    await motor_accelerate()
    accel_duration = time.ticks_ms() - start_accel
    print(f">>> 加速完成，耗时：{accel_duration} 毫秒")

    print(">>> 进入电机匀速阶段")
    start_time = time.ticks_ms()
    deceleration_done = [False] * 4
    decel_tasks = []

    while SATAE == 255:
        current_time = time.ticks_ms()
        await uasyncio.sleep_ms(10)

        # **实时检测 `SATAE` 变化**
        if SATAE != 255:
            print(">>> `SATAE` 状态变更，立即进入减速")
            decel_tasks = [uasyncio.create_task(motor_decelerate(i)) for i in range(4)]
            break

        for i in range(4):
            if not deceleration_done[i] and current_time - start_time >= cruise_time_ms[i]:  
                print(f">>> 电机 {i} 匀速结束，进入减速")
                decel_tasks.append(uasyncio.create_task(motor_decelerate(i)))
                deceleration_done[i] = True
        
        if all(deceleration_done):
            break

    # **等待所有电机减速完成**
    await uasyncio.gather(*decel_tasks)
    print(">>> 所有电机减速完成")

    # **最终强制停止所有电机**
    print(">>> 强制停止所有电机.")
    for p in pwm:
        p.duty_u16(stop_duty)  

    SATAE = 0
    print(SATAE)
    lcdupdate()


if __name__ == '__main__':
    uasyncio.run(main())