"""--------------------控制器初始化--------------------"""
from pymtn_lib20 import *

def DevInit():
    print("控制器初始化开始")
    ''' 搜索控制器'''
    (devNum, devinfolist, ret) = pymtn.NMC_DevSearch(pymtn.TSearchMode.Internet)
    if ret != 0:
        print("控制器搜索出错,返回值为:", ret)
        exit()
    if ret == 0:
        print("the count of devices: (%s)" % (devNum))
        for i in range(0, devNum):
            print(devinfolist[i].address)
            print(devinfolist[i].idStr)
            print(devinfolist[i].description)
            print(devinfolist[i].ID)
    if devNum <= 0:
        print("没有找到控制器")
        exit()

    ''' 打开第一个控制器'''
    (devHandle, ret) = pymtn.NMC_DevOpenByID(devinfolist[0].idStr)
    if ret != pymtn.RTN_CMD_SUCCESS:
        print("控制器打开出错", ret)
        exit()

    # ''' 打开第一个控制器--只有一张卡的时候，直接打开，不用搜索'''
    # (devHandle, ret) = pymtn.NMC_DevOpen(0)
    # if ret != pymtn.RTN_CMD_SUCCESS:
    #     print("控制器打开出错", ret)
    #     exit()

    (Info, ret) = pymtn.NMC_GetCardInfo(devHandle)
    num = Info.axisNum
    axisHandle = [c_ushort(0)] * num
    for i in range(0, num):
        (axisHandle[i], ret) = pymtn.NMC_MtOpen(devHandle, i)
        if ret != pymtn.RTN_CMD_SUCCESS:
            print("控制器打开出错", ret)
            exit()

    print("控制器初始化结束")
    return (devHandle, axisHandle)

"""--------------------电子齿轮--------------------"""
def _MtGear(devHandle, axisHandle):
    print("电子齿轮 测试开始")

    # 电子齿轮模式
    ret = pymtn.NMC_MtSetPrfMode(axisHandle[0], 7)
    if ret != 0:
        print(f"设置电子齿轮模式失败，错误码: {ret}")
    else:
        print("设置电子齿轮模式成功")

    # 设置Gear跟随方向
    # dir: 0-双向跟随, <0-负向跟随, >0-正向跟随
    ret = pymtn.NMC_MtGearSetDir(axisHandle[0], 0)  # 双向跟随
    if ret != 0:
        print(f"设置Gear跟随方向失败，错误码: {ret}")
    else:
        print("设置Gear跟随方向成功")


    # 设置Gear主轴参数
    # masterType: 1-AXIS规划值, 2-AXIS反馈值, 3-编码器值
    ret = pymtn.NMC_MtGearSetMaster(axisHandle[0], 1, 1)  # 主轴为0号轴，使用规划值
    if ret != 0:
        print(f"设置Gear主轴参数失败，错误码: {ret}")
    else:
        print("设置Gear主轴参数成功")


    # 设置Gear跟随倍率
    # masterEven: 主轴位移系数
    # slaveEven: 从轴位移系数
    # masterSlope: 主轴离合区位移（必须大于0且不等于1）
    masterEven = 1  # 主轴移动1个单位
    slaveEven = 2  # 从轴移动2个单位
    masterSlope = 10  # 离合区位移

    ret = pymtn.NMC_MtGearSetRatio(axisHandle[0], masterEven, slaveEven, masterSlope)
    if ret != 0:
        print(f"设置Gear跟随倍率失败，错误码: {ret}")
    else:
        print("设置Gear跟随倍率成功")


    # 启动Gear运动
    # 如果需要同步启动其他轴
    syncAxCnts = 0  # 同步启动轴数量
    syncAxArray = []  # 同步启动的轴号
    ret = pymtn.NMC_MtGearStartMtn(axisHandle[0], syncAxCnts, syncAxArray)
    if ret != 0:
        print(f"启动Gear运动失败，错误码: {ret}")
    else:
        print("启动Gear运动成功")

    print("电子齿轮 测试结束")
if __name__ == "__main__":
    devHandle, axisHandle = DevInit()
    _MtGear(devHandle, axisHandle[0])