using DG.Tweening;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Threading.Tasks;
using UnityEngine;
using UnityEngine.UIElements;

class MBOXCXRegAddress
{
    //MBOX CX Reg Address
    public static int CX_ID_MboxPlayControlWord = 0x0; //平台复位寄存器
}

//UDP指令的功能码
class UDPDataFunctionCode
{
    //Read Reg Object Channel Code
    public static int FunctionCode_Report_DnReg = 0x1001; //MBOX上报功能码
    public static int FunctionCode_Read_DnFnReg = 0x1101; //读寄存器功能码
    public static int FunctionCode_Write_DnFnReg = 0x1201; //写寄存器功能码
    public static int FunctionCode_AbsTimePlay = 0x1301; //绝对时间播放
    public static int FunctionCode_DeltaTimePlay = 0x1401; //相对时间播放
}

//UDP指令的通道号
class UDPDataObiectChannel
{
    //Read Reg Object Channel Code
    public static int ChannelCode_Read_DnReg = 0; //读Dn寄存器通道
    public static int ChannelCode_Read_FnReg = 1; //读Fn寄存器通道

    //Write Reg Object Channel Code
    public static int ChannelCode_Write_FnReg_NOSave = 0; //写Fn寄存器但掉电不保存通道
    public static int ChannelCode_Write_FnReg_Save = 1; //写Fn寄存器掉电保存通道
    public static int ChannelCode_Write_CXReg = 2; //写CX寄存器通道

    //Play Action Data（六轴数据支持1-6轴平台，建议在编程时直接使用6轴数据）
    public static int ChannelCode_Play_ActionData_Three = 0; //播放3轴数据通道
    public static int ChannelCode_Play_ActionData_Six = 1; //播放6轴数据通道
}

//UDP指令里的字偏移量
class UDPDataWordOffset
{
    //  UDP Tx Buffer WordOffset 
    public static int WordOffset000_UdpApp_ConfirmCode = 0;
    public static int WordOffset001_UdpApp_PassWord = 1;
    public static int WordOffset002_UdpApp_FunctionCode = 2;
    public static int WordOffset003_UdpApp_ObjectChannel = 3;
    public static int WordOffset004_UdpApp_WhoAcceptCode = 4;
    public static int WordOffset005_UdpApp_WhoReplyCode = 5;

    public static int WordOffset006_UdpApp_RegStartAddress = 6;
    public static int WordOffset007_UdpApp_RegVisitNum = 7;
    public static int WordOffset008_UdpApp_RegVisitDataBase = 8;
    public static int WordOffset009_UdpApp_RegVisitErrorCode = 9;

    public static int WordOffset006_UdpApp_PlayLineHigh = 6;
    public static int WordOffset007_UdpApp_PlayLineLow = 7;
    public static int WordOffset008_UdpApp_PlayTimeHigh = 8;
    public static int WordOffset009_UdpApp_PlayTimeLow = 9;

    public static int WordOffset010_UdpApp_PlayXPosHigh = 10;
    public static int WordOffset011_UdpApp_PlayXPosLow = 11;

    public static int WordOffset012_UdpApp_PlayYPosHigh = 12;
    public static int WordOffset013_UdpApp_PlayYPosLow = 13;

    public static int WordOffset014_UdpApp_PlayZPosHigh = 14;
    public static int WordOffset015_UdpApp_PlayZPosLow = 15;

    public static int WordOffset016_UdpApp_PlayUPosHigh = 16;
    public static int WordOffset017_UdpApp_PlayUPosLow = 17;

    public static int WordOffset018_UdpApp_PlayVPosHigh = 18;
    public static int WordOffset019_UdpApp_PlayVPosLow = 19;

    public static int WordOffset020_UdpApp_PlayWPosHigh = 20;
    public static int WordOffset021_UdpApp_PlayWPosLow = 21;

    public static int WordOffset022_UdpApp_PlayBaseDo = 22;
    public static int WordOffset023_UdpApp_PlayDac1 = 23;
    public static int WordOffset024_UdpApp_PlayDac2 = 24;
    public static int WordOffset025_UdpApp_PlayExtDo = 25;
}


class ELECTRIC_CYCLINDER
{
    public float F32AccessDistanceUnitMm = 50f; //电动缸行程
    public float F32LeadDistanceUnitMm = 5f; //电动缸导程
    public uint U32OneTurnPulseNum = 10000; //一圈脉冲数
    public float F32Gear = 1f; //减速比
}

// Udp App Data Structure 
class UDP_APP_DATA
{
    public short I16HostTxPort = 8410;
    public short I16HostRxPort = 8410;
    public short I16MboxTxPort = 7408;
    public short I16MboxRxPort = 7408;

    public ushort I16ConfirmCode = 0x55aa; //确认码
    public ushort I16PassCode = 0x0000; //通过码
    public ushort I16FunctionCode = 0x1401; //功能码：相对对时间码
    public ushort I16ChannelCode = 0x0001; //通道号：六轴模式

    public ushort I16WhoAcceptCode = 0xffff; //谁接收
    public ushort I16WhoReplyCode = 0xffff; //谁回复

    public int I32PlayLine = 0x00000000; //指令编号
    public int I32PlayTime = 0x00000000; //时间（单位：毫秒，1401相对时间为指令的执行时间，1301绝对时间下相邻两帧指令里的时间相减为执行时间）
    public int I32PlayXpos = 0x00000000; //第1轴位置信息
    public int I32PlayYpos = 0x00000000; //第2轴位置信息
    public int I32PlayZpos = 0x00000000; //第3轴位置信息
    public int I32PlayUpos = 0x00000000; //第4轴位置信息
    public int I32PlayVpos = 0x00000000; //第5轴位置信息
    public int I32PlayWpos = 0x00000000; //第6轴位置信息

    public ushort I16BaseDoutCode = 0x0000; //基础开关量输出

    public ushort I16DacOneCode = 0x0000; //第1路模拟量输出
    public ushort I16DacTwoCode = 0x0000; //第2路模拟量输出
    public ushort I16ExtDoutCode = 0x0000; //扩展12路模拟量输出
}

//Sine Parameter
class SINE_ACT_DATA
{
    public float F32SamplingPeriods = 0.01f;

    public float F32PeakMm = 50.0f;
    public float F32FrequencyHz = 0.1f;

    public float F32SineDataMm = 0.0f;

    public float F32XMm = 0.0f;
    public float F32YMm = 0.0f;
    public float F32ZMm = 0.0f;
    public float F32UMm = 0.0f;
    public float F32VMm = 0.0f;
    public float F32WMm = 0.0f;

    public short I16Delta = 0;
    public int I32Iine = 0;

    public int I32CyclePoint = 0;
}

public class MBOXManager : MonoBehaviour
{
    public UdpClient HostUDPPort; //打开8410端口
    public short[] I16UdpTxBuffer; //存放UDP数据信息 
    public byte[] U8UdpDataSend; //存放发送数据Byte信息    
    //定时器

    ELECTRIC_CYCLINDER ElectricCylinder;
    UDP_APP_DATA UdpAppData;
    SINE_ACT_DATA SineActData = new SINE_ACT_DATA();


    public static MBOXManager Instance;

    void Awake()
    {
        Instance = this;
    }

    /// <summary>
    /// X,U 右边
    /// Y,V 前面
    /// Z,W 左边 
    /// </summary>
    void Start()
    {
        UdpAppData = new UDP_APP_DATA();
        ElectricCylinder = new ELECTRIC_CYCLINDER();

        I16UdpTxBuffer = new short[25];
        U8UdpDataSend = new byte[50];


        UdpAppData.I16HostTxPort = (short)8413; //主机 发送UDP数据端口号
        UdpAppData.I16MboxRxPort = (short)7408; //MBOX 接收UDP数据端口号
        UdpAppData.I16WhoAcceptCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
        UdpAppData.I16WhoReplyCode = (ushort)ByteToWord((int)255, (int)255); //IP Group和IP Node合并为谁接收
        ElectricCylinder.F32AccessDistanceUnitMm = (float)33; //电动缸行程
        ElectricCylinder.F32LeadDistanceUnitMm = (float)5.00; //电动缸导程
        ElectricCylinder.U32OneTurnPulseNum = (uint)10000; //一圈脉冲数
        ElectricCylinder.F32Gear = (float)1.0000; //减速比
        HostUDPPort = new UdpClient(UdpAppData.I16HostTxPort);
        IPEndPoint ipendpoint = new IPEndPoint(IPAddress.Parse("192.168.15.201"), UdpAppData.I16MboxRxPort);
        HostUDPPort.Connect("255.255.255.255", UdpAppData.I16MboxRxPort);


        Debug.Log(HostUDPPort.Client);

        UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
        UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
        UdpAppData.I32PlayTime = 100; //时间单位为毫秒
        UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        PlayActionCmd();
    }

    /// <summary>
    /// 撞击
    /// </summary>
    public async void Bonk()
    {
        UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
        UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
        UdpAppData.I32PlayTime = 100; //时间单位为毫秒
        UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(20), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(20), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        PlayActionCmd();

        await Task.Delay(TimeSpan.FromSeconds(0.2f));

        UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
        UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
        UdpAppData.I32PlayTime = 100; //时间单位为毫秒
        UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(60), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(60), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        PlayActionCmd();
    }

    /// <summary>
    /// 左边
    /// </summary>
    /// <param name="val"></param>
    public void Left(float val)
    {
        if (val >= 20)
            return;

        UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
        UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
        UdpAppData.I32PlayTime = 100; //时间单位为毫秒
        UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        PlayActionCmd();
    }


    /// <summary>
    /// 右边
    /// </summary>
    /// <param name="val"></param>
    public void Right(float val)
    {
        if (val >= 20)
            return;
        UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
        UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
        UdpAppData.I32PlayTime = 100; //时间单位为毫秒
        UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm - val), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
        PlayActionCmd();
    }


    //public void A()
    //{
    //    Debug.Log((ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay);
    //    UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
    //    UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
    //    UdpAppData.I32PlayTime = 100;//时间单位为毫秒
    //    UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, (float)(ElectricCylinder.F32AccessDistanceUnitMm / 2.0), ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    PlayActionCmd();
    //}
    //public void B()
    //{
    //    UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
    //    UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
    //    UdpAppData.I32PlayTime = 100;//时间单位为毫秒
    //    UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    PlayActionCmd();
    //}

    //public void C()
    //{
    //    UdpAppData.I16FunctionCode = (ushort)UDPDataFunctionCode.FunctionCode_DeltaTimePlay;
    //    UdpAppData.I16ChannelCode = (ushort)UDPDataObiectChannel.ChannelCode_Play_ActionData_Six;
    //    UdpAppData.I32PlayTime = 100;//时间单位为毫秒
    //    UdpAppData.I32PlayXpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayYpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayZpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayUpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayVpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    UdpAppData.I32PlayWpos = (int)JogPulse(ElectricCylinder.F32Gear, ElectricCylinder.F32AccessDistanceUnitMm, ElectricCylinder.F32LeadDistanceUnitMm, ElectricCylinder.U32OneTurnPulseNum);
    //    PlayActionCmd();
    //}

    private void PlayActionCmd()
    {
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset000_UdpApp_ConfirmCode] = (short)Swap(UdpAppData.I16ConfirmCode); //确认码
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset001_UdpApp_PassWord] = (short)Swap(UdpAppData.I16PassCode); //通过码 
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset002_UdpApp_FunctionCode] = (short)Swap(UdpAppData.I16FunctionCode); //功能码 
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset003_UdpApp_ObjectChannel] = (short)Swap(UdpAppData.I16ChannelCode); //通道号
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset004_UdpApp_WhoAcceptCode] = (short)Swap(UdpAppData.I16WhoAcceptCode); //谁接收
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset005_UdpApp_WhoReplyCode] = (short)Swap(UdpAppData.I16WhoReplyCode); //谁回复
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset006_UdpApp_PlayLineHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayLine); // 指令编号
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset007_UdpApp_PlayLineLow] = (short)U32LowWord((uint)UdpAppData.I32PlayLine);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset008_UdpApp_PlayTimeHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayTime); //时间


        I16UdpTxBuffer[UDPDataWordOffset.WordOffset009_UdpApp_PlayTimeLow] = (short)U32LowWord((uint)UdpAppData.I32PlayTime);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset010_UdpApp_PlayXPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayXpos); //X位置
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset011_UdpApp_PlayXPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayXpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset012_UdpApp_PlayYPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayYpos); //Y位置
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset013_UdpApp_PlayYPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayYpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset014_UdpApp_PlayZPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayZpos); //Z位置


        I16UdpTxBuffer[UDPDataWordOffset.WordOffset015_UdpApp_PlayZPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayZpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset016_UdpApp_PlayUPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayUpos); //U位置 
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset017_UdpApp_PlayUPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayUpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset018_UdpApp_PlayVPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayVpos); //V位置
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset019_UdpApp_PlayVPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayVpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset020_UdpApp_PlayWPosHigh] = (short)U32HighWord((uint)UdpAppData.I32PlayWpos); //W位置
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset021_UdpApp_PlayWPosLow] = (short)U32LowWord((uint)UdpAppData.I32PlayWpos);
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset022_UdpApp_PlayBaseDo] = (short)Swap(UdpAppData.I16BaseDoutCode); //基本开关量
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset023_UdpApp_PlayDac1] = (short)Swap(UdpAppData.I16DacOneCode); // 模拟量1输出
        I16UdpTxBuffer[UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2] = (short)Swap(UdpAppData.I16DacTwoCode); // 模拟量2输出
        Buffer.BlockCopy(I16UdpTxBuffer, 0, U8UdpDataSend, 0, U8UdpDataSend.Length); // 将short数组转化为byte数组
        HostUDPPort.BeginSend(U8UdpDataSend, (UDPDataWordOffset.WordOffset024_UdpApp_PlayDac2 + 1) * 2, (state => { Debug.Log(state.IsCompleted); }), ""); //发送UDP数据
    }

    //将两个char数据合并为字节数据
    private int ByteToWord(int x, int y)
    {
        int z;
        z = ((x << 8) | y);
        return z;
    }

    //将short类型数据的高低位进行交换
    private ushort Swap(ushort x)
    {
        ushort highbit;
        ushort lowbit;
        ushort swap;
        highbit = (ushort)(x >> 8);
        lowbit = (ushort)((x & 0x00ff) << 8);
        swap = (ushort)(highbit | lowbit);
        return swap;
    }

    ///取Uint32的高１６位，并将Uint16的高８位和低８位交换
    private ushort U32HighWord(uint x)
    {
        ushort highbit;

        highbit = (ushort)(x >> 16);
        highbit = Swap(highbit);

        return (highbit);
    }

    ///取Uint32的低１６位，并将Uint16的高８位和低８位交换
    private ushort U32LowWord(uint x)
    {
        ushort lowbit;
        lowbit = (ushort)(x & 0x0000ffff);
        lowbit = Swap(lowbit);
        return (lowbit);
    }

    ///计算点动脉冲数
    private uint JogPulse(float Gear, float AcessDistance, float LeadDistance, uint OneTurnPulse)
    {
        uint NUM_Pulse;
        NUM_Pulse = (uint)(Gear * AcessDistance * OneTurnPulse / LeadDistance);
        return NUM_Pulse;
    }

    //正弦波形 
    private float SineDataMm(float peak, float frequency, float t)
    {
        float sinedata;

        sinedata = (float)((peak / 2) + (peak / 2) * Math.Sin(2 * Math.PI * frequency * t + 270 * Math.PI / 180));

        return sinedata;
    }
}