using System;
using System.Collections.Generic;
using System.Globalization;
using System.IO.Ports;

namespace Correlator.Util
{
    public static class CommandSender
    {
        /// <summary>
        /// 下发唤醒指令-加速度计
        /// </summary>
        /// <param name="sp"></param>
        public static void SendCorrelatorWakeUpCmd(SerialPort sp)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                byte[] btDevId = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; //广播模式，设备类型不解析

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0xFF, 0xFF };

                const short pduTypeByte = 2;
                var operateType = (short)(pduTypeByte & 0x7F);

                var pdu = (short)(2688 + operateType); //0X0A80
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] oid = { 0x30, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00 };

                byte[] totalLen = { 0x00, 0x13 };

                //wrap the whole data
                var result = new byte[1 + 1 + 2 + 6 + 1 + 2 + 2 + 1 + oid.Length];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oid.CopyTo(result, 16);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发加速度计数据采集指令 => " + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 下发唤醒指令-水听器
        /// </summary>
        /// <param name="sp"></param>
        public static void SendHydrophoneWakeUpCmd(SerialPort sp)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                //后续到CRC字段前的字节总长度(不包括长度字段和CRC字段)。
                byte[] totalLen = { 0x00, 0x1F };

                byte[] btDevId = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; //广播模式，设备类型不解析

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0xFF, 0xFF };

                const short pduTypeByte = 2;
                var operateType = (short)(pduTypeByte & 0x7F);

                var pdu = (short)(2688 + operateType); //0X0A80
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] oidTag =
                {
                    0x40, 0x00, 0x00, 0x01,
                    0x00, 0x01,
                    0x00
                };

                //获取时间（年-月-日-时-分-秒），年号-2000
                FlowStatus.HydrophoneWakeUpTimeBytes = new[]
                {
                    CalendarHub.Year, CalendarHub.Mon, CalendarHub.Day, CalendarHub.Hor, CalendarHub.Min, CalendarHub.Sec
                };
                byte[] timeTag =
                {
                    0x10, 0x00, 0x00, 0x51,
                    0x00, 0x06,
                    FlowStatus.HydrophoneWakeUpTimeBytes[0], FlowStatus.HydrophoneWakeUpTimeBytes[1],
                    FlowStatus.HydrophoneWakeUpTimeBytes[2], FlowStatus.HydrophoneWakeUpTimeBytes[3],
                    FlowStatus.HydrophoneWakeUpTimeBytes[4], FlowStatus.HydrophoneWakeUpTimeBytes[5]
                }; //时间oid

                //wrap the whole data
                var result = new byte[35];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oidTag.CopyTo(result, 16);
                timeTag.CopyTo(result, 23);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发水听器数据采集指令 => " + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 水听器点名指令
        /// </summary>
        /// <param name="sp"></param>
        /// <param name="devCode"></param>
        public static void StartCheckResponse(SerialPort sp, string devCode)
        {
            try
            {
                //A3 20
                //00 1F
                //21 17 00 08 22 02
                //01
                //FF FF
                //01 82
                //01
                //40 00 00 02 00 01 00
                //10 00 00 51 00 06 17 06 19 0F 3B 14

                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                //后续到CRC字段前的字节总长度(不包括长度字段和CRC字段)。
                byte[] totalLen = { 0x00, 0x1F };

                var btDevId = new byte[6];
                btDevId[0] = byte.Parse(devCode.Substring(0, 2), NumberStyles.HexNumber);
                btDevId[1] = byte.Parse(devCode.Substring(2, 2), NumberStyles.HexNumber);
                btDevId[2] = byte.Parse(devCode.Substring(4, 2), NumberStyles.HexNumber);
                btDevId[3] = byte.Parse(devCode.Substring(6, 2), NumberStyles.HexNumber);
                btDevId[4] = byte.Parse(devCode.Substring(8, 2), NumberStyles.HexNumber);
                btDevId[5] = byte.Parse(devCode.Substring(10, 2), NumberStyles.HexNumber);

                //通信方式
                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0xFF, 0xFF };

                byte[] btPdu = { 0x01, 0x82 };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] oidTag =
                {
                    0x40, 0x00, 0x00, 0x02,
                    0x00, 0x01,
                    0x00
                };

                byte[] timeTag =
                {
                    0x10, 0x00, 0x00, 0x51,
                    0x00, 0x06,
                    FlowStatus.HydrophoneWakeUpTimeBytes[0], FlowStatus.HydrophoneWakeUpTimeBytes[1],
                    FlowStatus.HydrophoneWakeUpTimeBytes[2], FlowStatus.HydrophoneWakeUpTimeBytes[3],
                    FlowStatus.HydrophoneWakeUpTimeBytes[4], FlowStatus.HydrophoneWakeUpTimeBytes[5]
                }; //时间oid

                //wrap the whole data
                var result = new byte[35];
                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oidTag.CopyTo(result, 16);
                timeTag.CopyTo(result, 23);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发水听器点名指令 => " + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 发送校验结果指令给下位机
        /// </summary>
        /// <param name="sp"></param>
        /// <param name="devCode"></param>
        /// <param name="indexArray">异常数据角标集合</param>
        public static void SendDataCheckResultCmd(SerialPort sp, string devCode, List<int> indexArray)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                //后续到CRC字段前的字节总长度(不包括长度字段和CRC字段)。
                byte[] totalLen = { 0x00, 0x26 };

                var btDevId = new byte[6];
                btDevId[0] = byte.Parse(devCode.Substring(0, 2), NumberStyles.HexNumber);
                btDevId[1] = byte.Parse(devCode.Substring(2, 2), NumberStyles.HexNumber);
                btDevId[2] = byte.Parse(devCode.Substring(4, 2), NumberStyles.HexNumber);
                btDevId[3] = byte.Parse(devCode.Substring(6, 2), NumberStyles.HexNumber);
                btDevId[4] = byte.Parse(devCode.Substring(8, 2), NumberStyles.HexNumber);
                btDevId[5] = byte.Parse(devCode.Substring(10, 2), NumberStyles.HexNumber);

                //通信方式
                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0xFF, 0xFF };

                byte[] btPdu = { 0x05, 0x82 };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                //check result tag
                var byteArray = indexArray.ToByteArray();
                byte[] oidTag =
                {
                    0x60, 0x00, 0x03, 0x00,
                    0x00, 0x08,
                    byteArray[0], byteArray[1], byteArray[2], byteArray[3],
                    byteArray[4], byteArray[5], byteArray[6], byteArray[7]
                };

                byte[] timeTag =
                {
                    0x10, 0x00, 0x00, 0x51,
                    0x00, 0x06,
                    FlowStatus.HydrophoneWakeUpTimeBytes[0], FlowStatus.HydrophoneWakeUpTimeBytes[1],
                    FlowStatus.HydrophoneWakeUpTimeBytes[2], FlowStatus.HydrophoneWakeUpTimeBytes[3],
                    FlowStatus.HydrophoneWakeUpTimeBytes[4], FlowStatus.HydrophoneWakeUpTimeBytes[5]
                }; //时间oid
                
                //wrap the whole data
                var result = new byte[42];
                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oidTag.CopyTo(result, 16);
                timeTag.CopyTo(result, 30);
                
                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发水听器检查指令 => " + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 下发状态采集指令
        /// </summary>
        /// <param name="devId"></param>
        /// <param name="sp"></param>
        public static void SendCellAndStatusCollectCmd(SerialPort sp, byte devId)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                byte[] btDevId = { 0x21, 0x17, 0x00, 0x08, 0x22, devId }; //广播模式，设备类型不解析

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0x22, devId };

                const short pduTypeByte = 2;
                var operateType = (short)(pduTypeByte & 0x7F);

                var pdu = (short)(2688 + operateType); //0X0A80
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] oid = { 0x60, 0x00, 0x02, 0x00, 0x00, 0x01, 0x00 };

                byte[] totalLen = { 0x00, 0x13 };

                var result = new byte[1 + 1 + 2 + 6 + 1 + 2 + 2 + 1 + oid.Length];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oid.CopyTo(result, 16);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("设备" + devId + " 下发状态采集指令 => " + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 下发听音指令
        /// </summary>
        /// <param name="devCode"></param>
        /// <param name="sp"></param>
        public static void SendSoundCollectCmd(SerialPort sp, string devCode)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                //byte[] btDevId = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; //广播模式，设备类型不解析
                var btDevId = new byte[6];
                btDevId[0] = byte.Parse(devCode.Substring(0, 2), NumberStyles.HexNumber);
                btDevId[1] = byte.Parse(devCode.Substring(2, 2), NumberStyles.HexNumber);
                btDevId[2] = byte.Parse(devCode.Substring(4, 2), NumberStyles.HexNumber);
                btDevId[3] = byte.Parse(devCode.Substring(6, 2), NumberStyles.HexNumber);
                btDevId[4] = byte.Parse(devCode.Substring(8, 2), NumberStyles.HexNumber);
                btDevId[5] = byte.Parse(devCode.Substring(10, 2), NumberStyles.HexNumber);

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                var dstNodeAddr = devCode.Substring(8); //2个字节 ** ****** ****
                var btDstNode = new byte[2];
                btDstNode[0] = byte.Parse(dstNodeAddr.Substring(0, 2), NumberStyles.HexNumber);
                btDstNode[1] = byte.Parse(dstNodeAddr.Substring(2, 2), NumberStyles.HexNumber);

                const short pduTypeByte = 2;
                var operateType = (short)(pduTypeByte & 0x7F);

                var pdu = (short)(2688 + operateType); //0X0A82Weakuprequest+z噪声记录仪
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                //第一个Tag的Oid为“0x30000001”，第二个Tag为时间戳，Oid为“0x10000051”，内容为“年、月、日、时、分、秒”
                byte[] oid = { 0x20, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00 }; //Tag1

                /*
                byte[] sysTag = { 0x10,0x00,0x00,0x51,0x00,0x06,
                    CalendarHub.YEAR, CalendarHub.MON, CalendarHub.DAY, CalendarHub.HOR, CalendarHub.MIN, CalendarHub.SEC };//Tag2
                */
                byte[] totalLen = { 0x00, 0x13 };

                //wrap the whole data
                var result = new byte[1 + 1 + 2 + 6 + 1 + 2 + 2 + 1 + oid.Length];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oid.CopyTo(result, 16);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发听音指令：" + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 下发听音停止指令
        /// </summary>
        /// <param name="devCode"></param>
        /// <param name="sp"></param>
        public static void SendSoundStopCmd(SerialPort sp, string devCode)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                var btDevId = new byte[6];
                btDevId[0] = byte.Parse(devCode.Substring(0, 2), NumberStyles.HexNumber);
                btDevId[1] = byte.Parse(devCode.Substring(2, 2), NumberStyles.HexNumber);
                btDevId[2] = byte.Parse(devCode.Substring(4, 2), NumberStyles.HexNumber);
                btDevId[3] = byte.Parse(devCode.Substring(6, 2), NumberStyles.HexNumber);
                btDevId[4] = byte.Parse(devCode.Substring(8, 2), NumberStyles.HexNumber);
                btDevId[5] = byte.Parse(devCode.Substring(10, 2), NumberStyles.HexNumber);

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                var dstNodeAddr = devCode.Substring(8); //2个字节 ** ****** ****
                var btDstNode = new byte[2];
                btDstNode[0] = byte.Parse(dstNodeAddr.Substring(0, 2), NumberStyles.HexNumber);
                btDstNode[1] = byte.Parse(dstNodeAddr.Substring(2, 2), NumberStyles.HexNumber);

                const short pduTypeByte = 2;
                const short operateType = pduTypeByte & 0x7F;

                const short pdu = 1408 + operateType; //0x0582 TrapResponse+z噪声记录仪
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] sysTag =
                {
                    0x10, 0x00, 0x00, 0x51, 0x00, 0x06,
                    CalendarHub.Year, CalendarHub.Mon, CalendarHub.Day, CalendarHub.Hor, CalendarHub.Min, CalendarHub.Sec
                }; //Tag2

                byte[] totalLen = { 0x00, 0x18 };

                //wrap the whole data
                var result = new byte[1 + 1 + 2 + 6 + 1 + 2 + 2 + 1 + sysTag.Length];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                sysTag.CopyTo(result, 16);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发听音结束指令：" + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }

        /// <summary>
        /// 下发测距指令
        /// </summary>
        /// <param name="sp"></param>
        public static void SendCorrelatorRangingCmd(SerialPort sp)
        {
            try
            {
                const string preamble = "A3";
                var preambleByte = byte.Parse(preamble, NumberStyles.HexNumber);

                const string version = "20";
                var btVersion = byte.Parse(version, NumberStyles.HexNumber);

                byte[] btDevId = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; //广播模式，设备类型不解析

                const string routeFlag = "1";
                var btRouteFlag = byte.Parse(routeFlag, NumberStyles.HexNumber);

                byte[] btDstNode = { 0xFF, 0xFF };

                const short pduTypeByte = 2;
                const short operateType = pduTypeByte & 0x7F;

                const short pdu = 2688 + operateType; //0X0A80
                var btPdu0 = BitConverter.GetBytes(pdu);
                byte[] btPdu = { btPdu0[1], btPdu0[0] };

                const string seq = "1";
                var btSeq = byte.Parse(seq, NumberStyles.HexNumber);

                byte[] oid = { 0x30, 0x00, 0x00, 0x01, 0x00, 0x01, 0x99 };

                byte[] totalLen = { 0x00, 0x13 };

                //wrap the whole data
                var result = new byte[1 + 1 + 2 + 6 + 1 + 2 + 2 + 1 + oid.Length];

                result[0] = preambleByte;
                result[1] = btVersion;
                totalLen.CopyTo(result, 2);
                btDevId.CopyTo(result, 4);
                result[10] = btRouteFlag;
                btDstNode.CopyTo(result, 11);
                btPdu.CopyTo(result, 13);
                result[15] = btSeq;
                oid.CopyTo(result, 16);

                //增加CRC校验
                var strCrc = $"{(int)CodeUtil.CRC16_AD(result):X}".To16HexString();
                byte[] crcByte =
                {
                    CodeUtil.String2Byte(strCrc.Substring(0, 2)),
                    CodeUtil.String2Byte(strCrc.Substring(2, 2))
                };

                var afCrc = new byte[result.Length + 2];
                result.CopyTo(afCrc, 0);
                crcByte.CopyTo(afCrc, result.Length);

                "CommandSender".WriteLog("下发相关仪测距指令" + BitConverter.ToString(afCrc));

                sp.Write(afCrc, 0, afCrc.Length);
            }
            catch (Exception e)
            {
                "CommandSender".WriteLog(e.ToString());
            }
        }
    }
}