"""--------------------控制器初始化--------------------"""
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 _MtFollow(devHandle, axisHandle):
    print("电子凸轮 测试开始")

    # Follow跟随模式
    ret = pymtn.NMC_MtSetPrfMode(axisHandle[0], 8)
    if ret != 0:
        print(f"设置Follow跟随模式失败，错误码: {ret}")
    else:
        print("设置Follow跟随模式成功")

    fifoNo = 1
    # 清除FIFO数据（可选）
    ret = pymtn.NMC_MtFollowClear(axisHandle[0], fifoNo)
    if ret != 0:
        print(f"清除FIFO数据失败，错误码: {ret}")
    else:
        print("清除FIFO数据成功")

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


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

    #  获取FOLLOW FIFO剩余空间
    fifoNo = 0  # FIFO 0
    space_tmp, ret = pymtn.NMC_MtFollowGetSpace(axisHandle[0], fifoNo)
    if ret != 0:
        print(f"获取FIFO空间失败，错误码: {ret}")
    else:
        print(f"FIFO {fifoNo} 剩余空间: {space_tmp.value}")

    # 向FIFO推送跟随数据
    # 段类型: 0-普通段, 1-匀速段, 2-停止段, 3-连续段

    # 推送普通段数据
    ret = pymtn.NMC_MtFollowPushData(axisHandle[0], 1000, 500.0, 0, fifoNo)  # 主轴1000，从轴500
    if ret != 0:
        print(f"推送FOLLOW数据失败，错误码: {ret}")
    else:
        print("推送FOLLOW数据成功")

    # 推送匀速段数据
    ret = pymtn.NMC_MtFollowPushData(axisHandle[0], 2000, 1000.0, 1, fifoNo)  # 主轴2000，从轴1000
    if ret != 0:
        print(f"推送匀速段数据失败，错误码: {ret}")
    else:
        print("推送匀速段数据成功")


    # 设置FOLLOW循环执行次数
    ret = pymtn.NMC_MtFollowSetLoopCount(axisHandle[0], 0)  # 设置为无限循环
    if ret != 0:
        print(f"设置FOLLOW循环次数失败，错误码: {ret}")
    else:
        print("设置FOLLOW循环次数成功")


    # 设置FOLLOW启动事件
    # eventType: 1-立即启动, 2-主轴穿越设定位置后启动
    # masterDir: 1-主轴正向运动, -1-主轴负向运动
    # pos: 穿越位置
    ret = pymtn.NMC_MtFollowSetEvent(axisHandle[0], 1, 1, 1000)  # 立即启动，正向，位置1000
    if ret != 0:
        print(f"设置FOLLOW启动事件失败，错误码: {ret}")
    else:
        print("设置FOLLOW启动事件成功")

    # 启动FOLLOW运动
    syncAxCnts = 0
    syncAxArray = []  # 创建c_short数组
    ret = pymtn.NMC_MtFollowStart(axisHandle[0], syncAxCnts, syncAxArray)
    if ret != 0:
        print(f"启动FOLLOW运动失败，错误码: {ret}")
    else:
        print("启动FOLLOW运动成功")


    print("12.电子凸轮 测试结束")


if __name__ == "__main__":

    devHandle, axisHandle = DevInit()
    _MtFollow(devHandle, axisHandle[0])