from ULAN.UBusCommLibWrapper import *
import time
import sys

mFlexrayClusterParameter = FlexrayClusterParameter_t(
    10, 7, 91, 1, 10, 5, 50, 0, 11, 59, 55, 301, 180, 60, 2, 0, 5000, 8, 6, 90, 80, 4985, 10, 10)
mFlexrayClusterParameter_ptr = ctypes.pointer(mFlexrayClusterParameter)

mFlexrayNodeParameter = FlexrayNodeParameter_t(
    401202, 9, 9, 16, 1, 0, 2, 212, 1, 1, 1, 1, 1, 79, 23, 23, 200000, 601, 40, 601, 141, 0, 44, 56, 0, 0)
mFlexrayNodeParameter_ptr = ctypes.pointer(mFlexrayNodeParameter)

mFRPhyNodeKeySlotFlat = FRPhyNodeKeySlotFlat_t( 40, 1, 1, 41, 1, 1 )
mFRPhyNodeKeySlotFlat_ptr = ctypes.pointer(mFRPhyNodeKeySlotFlat)

mFlerayFifoParameterFlat   = FlerayFifoParameterFlat()
# 初始化 FlexrayCCParameter_t 对象
mFlexrayCCParameter = FlexrayCCParameter_t(
    0,  # uint8_t freezeMode
    1,  # uint8_t channelA
    1,  # uint8_t channelB
    0,  # uint8_t syncFrameFilter
    0   # uint8_t bitRate (UFR_BITRATE_10M)
)
mFlexrayCCParameter_ptr = ctypes.pointer(mFlexrayCCParameter)
# 初始化 FlexrayMsgBufferParameter_t 对象
mFlexrayMsgBufferParameter = FlexrayMsgBufferParameter_t(
    MBID_DEFINE_BY_DEVICE,  # MessageBufferID_t msgBufferID
    31,  # uint16_t frameID
    0,   # uint8_t PPI
    10,  # uint8_t payloadLength
    1,   # uint8_t isTx
    1,   # uint8_t channelA
    1,   # uint8_t channelB
    0,   # uint8_t baseCycle
    1,   # uint8_t repetitionCycle
    0    # uint8_t repeatTx
)
gFlexrayMsgBufferParameter = [
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 1, 0, 10, 1, 1, 1, 0, 1, 0),   # 发送时隙
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 2, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 3, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 4, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 5, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 6, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 7, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 8, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 9, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 10, 0, 10, 1, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 21, 0, 10, 0, 1, 1, 0, 1, 0),  # 接收时隙
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 22, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 23, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 24, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 25, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 26, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 27, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 28, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 29, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 30, 0, 10, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 91, 0, 16, 0, 1, 1, 0, 1, 0),  # 接收动态时隙
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 92, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 93, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 94, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 95, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 96, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 97, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 98, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 99, 0, 16, 0, 1, 1, 0, 1, 0),
    FlexrayMsgBufferParameter_t(MBID_DEFINE_BY_DEVICE, 100, 0, 16, 0, 1, 1, 0, 1, 0),
]
# gFlexrayMsgBufferParameter1_ptr = ctypes.pointer(gFlexrayMsgBufferParameter[1])
# gFlexrayMsgBufferParameter2_ptr = ctypes.pointer(gFlexrayMsgBufferParameter[2])
# gFlexrayMsgBufferParameter3_ptr = ctypes.pointer(gFlexrayMsgBufferParameter[3])
gFlexrayMsgBufferParameter_ptr =[]
for i in range(30):
    gFlexrayMsgBufferParameter_ptr.append(ctypes.pointer(gFlexrayMsgBufferParameter[i]))

# gFlexrayMsgBufferParameter = [
#     (MBID_DEFINE_BY_DEVICE, 1, 0, 10, 1, 1, 1, 0, 1, 0),   # 发送时隙
#     (MBID_DEFINE_BY_DEVICE, 2, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 3, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 4, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 5, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 6, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 7, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 8, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 9, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 10, 0, 10, 1, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 21, 0, 10, 0, 1, 1, 0, 1, 0),  # 接收时隙
#     (MBID_DEFINE_BY_DEVICE, 22, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 23, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 24, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 25, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 26, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 27, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 28, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 29, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 30, 0, 10, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 91, 0, 16, 0, 1, 1, 0, 1, 0),  # 接收动态时隙
#     (MBID_DEFINE_BY_DEVICE, 92, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 93, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 94, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 95, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 96, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 97, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 98, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 99, 0, 16, 0, 1, 1, 0, 1, 0),
#     (MBID_DEFINE_BY_DEVICE, 100, 0, 16, 0, 1, 1, 0, 1, 0),
# ]

def Sleep(msec):
    sec = msec / 1000.0
    time.sleep(sec)

if __name__ == '__main__':

    # 设置目标 IP 地址和端口号
    target_ip = "192.168.0.8"
    m_UBusCommLibWrapper = UBusCommLibWrapper()

    # 打开设备
    instance = m_UBusCommLibWrapper.OpenDevice(target_ip)
    if instance <= 0:
        print("Open flexray device fail!")
        print(instance)
        exit(0)
    print("Open flexray device OK!")
    print(instance)

    # 配置cluster参数
    if m_UBusCommLibWrapper.Set_Cluster_Parameter(instance, 0, mFlexrayClusterParameter_ptr) != CMD_SUCCESS:
        print("Set_Cluster_Parameter fail!")
        exit(0)
    print("Set_Cluster_Parameter OK!")

    # 配置节点参数
    if m_UBusCommLibWrapper.Set_Node_Parameter(instance, 0, mFlexrayNodeParameter_ptr) != CMD_SUCCESS:
        print("UFr_Set_Node_Parameter fail!")
        exit(0)
    print("UFr_Set_Node_Parameter OK!")

    # 配置CC参数
    if m_UBusCommLibWrapper.Set_CC_Parameter(instance, 0, mFlexrayCCParameter_ptr) != CMD_SUCCESS:
        print("UFr_Set_CC_Parameter fail!")
        exit(0)
    print("UFr_Set_CC_Parameter OK!")

    # # 配置发送时隙参数
    if m_UBusCommLibWrapper.Set_Slot_Parameter(instance, 0, gFlexrayMsgBufferParameter_ptr[1]) != CMD_SUCCESS:
        print("UFr_Set_Slot_Parameter FAIL(RX)!")
        exit(0)
    print(f"UFr_Set_Slot_Parameter ok[RX][used msgBufferID = {gFlexrayMsgBufferParameter[1].msgBufferID}]!")
    # 配置接收时隙参数
    if m_UBusCommLibWrapper.Set_Slot_Parameter(instance, 0, gFlexrayMsgBufferParameter_ptr[10]) != CMD_SUCCESS:
        print(f"UFr_Set_Slot_Parameter FAIL(RX)!")
        exit(0)
    print(f"UFr_Set_Slot_Parameter ok[RX][used msgBufferID = {gFlexrayMsgBufferParameter[10].msgBufferID}]!")

# #设置两个KeySlots,建立FlexRay网络
#     ret = m_UBusCommLibWrapper.SetFrPhyNodeKeySlot(instance, 0, mFRPhyNodeKeySlotFlat_ptr)
#     if ret :
#         print("SetFrPhyNodeKeySlot failed")
#     print(f"SetFrPhyNodeKeySlot failed {ret}")

    # 配置使能
    if m_UBusCommLibWrapper.Configure_Node(instance, 0) != CMD_SUCCESS:
        print("UFr_Configure_Node fail!")
        sys.exit(0)
    print("UFr_Configure_Node OK!")
    
    # 启动接收数据上传操作
    if m_UBusCommLibWrapper.StartFrRxUpload(instance, 0) != CMD_SUCCESS:
        print("UFr_StartFrRxUpload fail!")
        sys.exit(0)
    print("UFr_StartFrRxUpload OK! ")

    # 启动节点，进入总线建立连接状态
    ret = m_UBusCommLibWrapper.Start_Node(instance, 0)
    if ret != CMD_SUCCESS:
        print(f"UFr_Start_Node fail[{ret}]!")
        sys.exit(0)
    print("UFr_Start_Node OK!")

    while True:
        # 获得节点的状态
        # frnodeState, curCycle = pyuflex.FlexRayNodeState(), pyuflex.CycleCount()
        frnodeState = ctypes.c_int8(0)
        frnodeState_ptr = ctypes.pointer(frnodeState)
        curCycle = ctypes.c_int8(0)
        curCycle_ptr = ctypes.pointer(curCycle)
        m_UBusCommLibWrapper.GetFrNodeState(instance, 0, frnodeState_ptr, curCycle_ptr)
        frnodeState = frnodeState.value
        while True:
            if frnodeState == FR_POCSTATE_DEFAULT_CONFIG:
                print("FlexRay Node is in status [defconfig]")
                break
            if frnodeState == FR_POCSTATE_CONFIG:
                print("FlexRay Node is in status [config]")
                break
            if frnodeState == FR_POCSTATE_WAKEUP:
                print("FlexRay Node is in status [wakeup]")
                break
            if frnodeState == FR_POCSTATE_READY:
                print("FlexRay Node is in status [ready]")
                break
            if frnodeState == FR_POCSTATE_NORMAL_ACTIVE:
                print("FlexRay Node is in status [normal active]")
                break
            if frnodeState == FR_POCSTATE_NORMAL_PASSIVE:
                print("FlexRay Node is in status [normal passive]")
                break
            if frnodeState == FR_POCSTATE_HALT:
                print("FlexRay Node is in status [halt]")
                break
            if frnodeState == FR_POCSTATE_STARTUP:
                print("FlexRay Node is in status [startup]")
                break

        if frnodeState == FR_POCSTATE_NORMAL_ACTIVE:
            print("FlexRay bus connect created! You can start recieve and Transmit!")
            break
        # print(f"FlexRay Node is in status [{frnodeState}]")
        time.sleep(1)
        # 如果有按键退出程序，退出程序前应stop和close设备，否则需要重新启动设备才能进行配置
        # if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
        #     line = input()
        #     if pyuflex.UFr_Stop_Wrapper(instance, 0) != CMD_SUCCESS:
        #         print("UFr_Stop fail!")
        #     if pyuflex.UFr_Close_Wrapper(instance) != CMD_SUCCESS:
        #         print("UFr_Close fail!")
        #     print("Device is closed, Press Enter to Exit!")
        #     sys.exit(0)
    # 发送一帧数据
    mFlexrayFrame = uFlexrayFrame_t()
    mFlexrayFrame.payloadLength = mFlexrayClusterParameter.gPayloadLengthStatic


    # print(mFlexrayFrame.payloadLength)
    for i in range(mFlexrayFrame.payloadLength):
        mFlexrayFrame.data[i] = 0x1234 + i

    mFlexrayFrame_data_ptr = (ctypes.c_uint16 * len(mFlexrayFrame.data))(*mFlexrayFrame.data)
    if m_UBusCommLibWrapper.Transmit(instance, 0, gFlexrayMsgBufferParameter[1].msgBufferID, mFlexrayFrame_data_ptr,#用id()传入指针无法改变参数的值
                                    mFlexrayFrame.payloadLength) != CMD_SUCCESS:
        print("UFr_Transmit fail!")
    else:
        print("UFr_Transmit OK!")


#清理缓存
    # if m_UBusCommLibWrapper.ClearRcvBuffer(instance, 0) != CMD_SUCCESS:
    #     print("UFr_ClearRcvBuffer fail!")
    # else:
    #     print("UFr_ClearRcvBuffer OK!")

    # 接收并处理数据
    rcvcount = 0
    signal3 = 0.01
    # 借用C库建立可被C动态库调用的数组
    RcvArray = ctypes.c_uint16*127
    rcvData = RcvArray()

    while True:
        uFlexrayFrameHead = uFlexrayFrameHead_t()
        uFlexrayFrameHead_ptr = ctypes.pointer(uFlexrayFrameHead)
        ret = m_UBusCommLibWrapper.Receive(instance, uFlexrayFrameHead_ptr, rcvData, 1000)
        # print(ret)
        if ret == CMD_SUCCESS:
            rcvcount += 1
            print(
                f"sn={uFlexrayFrameHead.serial_number} time={uFlexrayFrameHead.timestamp_s}.{uFlexrayFrameHead.timestamp_us} channel={uFlexrayFrameHead.Channel} ID={uFlexrayFrameHead.frameID} Cycle={uFlexrayFrameHead.cycleCount} Length={uFlexrayFrameHead.payloadLength} slotstatus={uFlexrayFrameHead.slotStatus} startup={uFlexrayFrameHead.startupFrameIndicator} sync={uFlexrayFrameHead.syncFrameIndicator} NFI={uFlexrayFrameHead.nullFrameIndicator} PPI={uFlexrayFrameHead.payloadPreambleIndicator} headCRC={uFlexrayFrameHead.headerCrc} data:",
                end=" ")
            for i in range(uFlexrayFrameHead.payloadLength):
                print(f"{rcvData[i]:04X}", end=" ")
            # 将接收到的数据部分通过发送时隙发送出去
            if m_UBusCommLibWrapper.Transmit(instance, 0, gFlexrayMsgBufferParameter[1].msgBufferID, rcvData,
                                            uFlexrayFrameHead.payloadLength) != CMD_SUCCESS:
                print("UFr_Transmit fail!")
            else:
                print("UFr_Transmit OK!")
        time.sleep(0.01)
        # 如果退出程序，退出程序前应stop和close设备，否则需要重新启动设备才能进行配置, 该Demo为了做演示没有关闭设备，使用者请在实践工程的适当位置关闭
        #     ret = m_UBusCommLibWrapper.StopFrRxUpload(instance, 0)
        #     if ret != CMD_SUCCESS:
        #         printf("UFr_StopFrRxUpload fail!")
        #     ret = m_UBusCommLibWrapper.Stop(instance, 0)
        #     if ret != CMD_SUCCESS:
        #         printf("UFr_Stop fail!")
        #     ret = m_UBusCommLibWrapper.CloseDevice(instance)
        #     if ret != CMD_SUCCESS:
        #         printf("UFr_Close fail!")
