using Plane.Copters;
using Plane.Geography;
using Plane.Protocols;
using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media;
using static Plane.Copters.PlaneCopter;
using static Plane.Protocols.MAVLink;

namespace Plane.CommunicationManagement
{
    public partial class CommModuleManager
    {
        public int packetcount = 0;
        private static object lock_rtcm = new object();//互锁量
        private List<byte[]> rtcm_packets = new List<byte[]>();
        private DateTime waitRtcmTime = DateTime.Now;
        private bool starttime = false;
        private bool rtcm_run = false;
        private SerialPort RecomPort;
      //  public bool Recomisopen = false;
        private bool _Recomisopen = false;
        public bool Recomisopen
        {
            get { return _Recomisopen; }
            set
            {
                if (_Recomisopen != value)
                {
                    _Recomisopen = value;
                    //_RtcmInfoViewModel.SetRTKStatestr();
                }
           }
        }



        private IEnumerable<ICopter> _allcopters;
        //是否使用专用传输模块
        public bool UseTransModule = true;

        private byte[] SetChannels(ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null, ushort? ch5 = null, ushort? ch6 = null, ushort? ch7 = null, ushort? ch8 = null)
        {
            MAVLink.mavlink_rc_channels_override_t rc = new MAVLink.mavlink_rc_channels_override_t();
            rc.target_component = 1;
            rc.target_system = 1;
            rc.chan1_raw = ch1 ?? 1500;
            rc.chan2_raw = ch2 ?? 1500;
            rc.chan3_raw = ch3 ?? 1500;
            rc.chan4_raw = ch4 ?? 1500;
            rc.chan5_raw = 1500;
            rc.chan6_raw = 1500;
            rc.chan7_raw = 1500;
            rc.chan8_raw = 1500;
            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, rc);
        }

        private byte[] SetModeAsync(FlightMode mode)
        {
            return SetModeAsync(mode.ToAC2Mode());
        }

        private byte[] DoARMAsync(bool armit)
        {
            return DoCommandAsync(MAVLink.MAV_CMD.COMPONENT_ARM_DISARM, armit ? 1 : 0, 21196, 0, 0, 0, 0, 0);
        }

        private byte[] DoMotorTestAsync(int motor, MAVLink.MOTOR_TEST_THROTTLE_TYPE thr_type, int throttle, int timeout)
        {
            return DoCommandAsync(MAVLink.MAV_CMD.DO_MOTOR_TEST, (float)motor, (float)(byte)thr_type, (float)throttle, (float)timeout, 0, 0, 0);
        }

        private byte[] DoCommandAsync(MAVLink.MAV_CMD actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {
            MAVLink.mavlink_command_long_t req = new MAVLink.mavlink_command_long_t();

            req.target_system = 1;
            req.target_component = 1;
            if (actionid == MAVLink.MAV_CMD.COMPONENT_ARM_DISARM)
            {
                req.target_component = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;
            }
            req.command = (ushort)actionid;
            req.param1 = p1;
            req.param2 = p2;
            req.param3 = p3;
            req.param4 = p4;
            req.param5 = p5;
            req.param6 = p6;
            req.param7 = p7;

            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_LONG, req); ;
        }

        private byte[] GeneratePacket<TMavlinkPacket>(byte messageType, TMavlinkPacket indata)
        {
            byte[] data;
            data = MavlinkUtil.StructureToByteArray(indata);
            byte[] packet = new byte[data.Length + 6 + 2];
            packet[0] = MAVLink.MAVLINK_STX;
            packet[1] = (byte)(data.Length);
            packet[2] = (byte)packetcount;
            packetcount++;
            packet[3] = 255; // this is always 255 - MYGCS
            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
            packet[5] = messageType;

            int i = 6;
            foreach (byte b in data)
            {
                packet[i] = b;
                i++;
            }
            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);
            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8); ///< Low byte
            packet[i] = ck_a;
            i += 1;
            packet[i] = ck_b;
            i += 1;

            return packet;
        }


        private byte[] SetModeAsync(ac2modes modein)
        {
            try
            {
                MAVLink.mavlink_set_mode_t mode = new MAVLink.mavlink_set_mode_t();

                if (TranslateMode(modein, ref mode))
                {
                    return SetModeAsync(mode);
                }
            }
            catch { }
            return null;
        }

        private byte[] SetModeAsync(MAVLink.mavlink_set_mode_t mode, MAVLink.MAV_MODE_FLAG base_mode = 0)
        {
            mode.base_mode |= (byte)base_mode;
            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_SET_MODE, mode);
        }

        private bool TranslateMode(ac2modes modein, ref MAVLink.mavlink_set_mode_t mode)
        {
            mode.target_system = 1;
            switch (modein)
            {
                case ac2modes.STABILIZE:
                case ac2modes.AUTO:
                case ac2modes.RTL:
                case ac2modes.LOITER:
                case ac2modes.ACRO:
                case ac2modes.ALT_HOLD:
                case ac2modes.CIRCLE:
                case ac2modes.POSITION:
                case ac2modes.LAND:
                case ac2modes.OF_LOITER:
                case ac2modes.GUIDED:
                    mode.base_mode = (byte)MAVLink.MAV_MODE_FLAG.CUSTOM_MODE_ENABLED;
                    mode.custom_mode = (uint)modein;
                    break;
                default:
                    return false;
            }
            return true;
        }

        private byte[] SetParam2Async(string paramname, float value)
        {
            // param type is set here, however it is always sent over the air as a float 100int = 100f.
            var req = new MAVLink.mavlink_param_set_t { target_system = 1, target_component = 1, param_type = 4 };

            byte[] temp = Encoding.GetEncoding("ASCII").GetBytes(paramname);

            Array.Resize(ref temp, 16);
            req.param_id = temp;
            req.param_value = (value);

            return GeneratePacket((byte)MAVLink.MAVLINK_MSG_ID_PARAM_SET, req);
        }

        private byte[] GetParam2Async(string paramname)
        {
            var paramId = Encoding.ASCII.GetBytes(paramname);
            Array.Resize(ref paramId, 16);

            var req = new MAVLink.mavlink_param_request_read_t
            {
                param_index = -1,
                target_system = 1,
                target_component = 1,
                param_id = paramId
            };

            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_PARAM_REQUEST_READ, req);
        }

        private byte[] DoLEDCommandAsync(byte red,byte green,byte blue)
        {
            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
            led.target_system = 1;
            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
            led.instance = 0;
            led.pattern = 0;
            led.custom_len = 4;
            led.custom_bytes = new byte[24];
            led.custom_bytes[0] = red;
            led.custom_bytes[1] = green;
            led.custom_bytes[2] = blue;
            led.custom_bytes[3] = 3;  //持续亮的时间=1/3hz=0.33s
            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
        }
        private byte[] DoThrowoutCommandAsync()
        {
            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
            led.target_system = 1;
            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
            led.instance = 0;
            led.pattern = 0;
            led.custom_len = 1;                //长度是1个字节
            led.custom_bytes = new byte[24];
            led.custom_bytes[0] = (byte)10;  //10代表抛物
            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
        }


        private byte[] DoTaskLEDCommandAsync(int ledmode, float ledInterval, int ledtimes, byte cR,byte cG,Byte cB)
        {
            //新版固件ledInterval间隔单位秒，传输为一个字节无法传输小数，所以*10改为100ms，可传输一位小数
            byte LEDInterval = (byte)(ledInterval * 10);// (byte)(10 / ledrate);
            MAVLink.mavlink_msg_id_led_control led = new MAVLink.mavlink_msg_id_led_control();
            led.target_system = 1;
            led.target_component = 1;//(byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_SYSTEM_CONTROL;;
            led.instance = 0;
            led.pattern = 0;
            led.custom_len = 6;  //测试LED灯光
            led.custom_bytes = new byte[24];
            led.custom_bytes[0] = cR;
            led.custom_bytes[1] = cG;
            led.custom_bytes[2] = cB;
            led.custom_bytes[3] =(byte)ledmode;
            led.custom_bytes[4] = LEDInterval;
            led.custom_bytes[5] = (byte)ledtimes;
            return GeneratePacket(MAVLink.MAVLINK_MSG_ID_LED_CONTROL, led);
        }



        private byte[] BatchCopterIdData(IEnumerable<ICopter> copters)
        {
            byte[] BatchByte = null;
            List<ICopter> copterList = copters.OrderBy(c => int.Parse(c.Id)).ToList();
            for (int i = 0; i < copterList.Count; i++)
            {
                byte[] tempByte;
                int id = int.Parse(copterList[i].Id);
                int count = 1;
                while (i + 1 < copterList.Count && int.Parse(copterList[i + 1].Id) == id + count)
                {
                    count++;
                    i++;
                }

                if (count == 1)
                {
                    short curId = (short)(0 << 12 ^ id);
                    tempByte = BitConverter.GetBytes(curId);
                }
                else
                {
                    short beginId = (short)(2 << 12 ^ id);
                    tempByte = BitConverter.GetBytes(beginId);

                    short numCount = (short)(4 << 12 ^ count);
                    tempByte = tempByte.Concat(BitConverter.GetBytes(numCount)).ToArray();
                }

                if (BatchByte == null)
                    BatchByte = tempByte;
                else
                    BatchByte = BatchByte.Concat(tempByte).ToArray();
            }
            return BatchByte;
        }

        private void GetCopterIds(IEnumerable<ICopter> copters, out short copterId, out byte[] batchPacket)
        {
            copterId = 0;
            batchPacket = null;

            if (copters != null)
            {
                if (copters.Count() == 1)
                    copterId = short.Parse(copters.FirstOrDefault().Id);
                else
                {
                    batchPacket = BatchCopterIdData(copters);
                }
            }
        }

        private async Task RtcmLoop()
        {
            while (rtcm_run)
            {
                byte[] sendDate = new byte[0];
                lock (lock_rtcm)
                {
                    if (rtcm_packets.Count >= 3 ||
                        (starttime && waitRtcmTime.AddMilliseconds(200) <= DateTime.Now))
                    {
                        if (rtcm_packets.Count > 0)
                        {
                            Windows.Messages.Message.Show($"rtcm_packets.Count = {rtcm_packets.Count}");
                            for (int i = 0; i < rtcm_packets.Count; i++)
                            {
                                sendDate = sendDate.Concat(rtcm_packets[i]).ToArray();
                            }
                            rtcm_packets.Clear();
                            starttime = false;
                        }
                    }
                }

                if (sendDate != null && sendDate.Length > 0)
                {
                    Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 通信数据长度 = {sendDate.Length}");
                    await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, sendDate).ConfigureAwait(false);
                }

                await Task.Delay(10);
            }
        }

        /////////////////////////////////////////////////////////////公开输出函数
        /// <summary>
        /// 设置通道
        /// </summary>
        /// <param name="id"></param>
        /// <param name="ch1"></param>
        /// <param name="ch2"></param>
        /// <param name="ch3"></param>
        /// <param name="ch4"></param>
        /// <returns></returns>



        public async Task SetChannelsAsync(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
        {
            if (UseTransModule)
                await SetChannelsAsync_CMod(copters, ch1, ch2, ch3, ch4);
            else await SetChannelsAsync_Single(copters, ch1, ch2, ch3, ch4);
        }
        public async Task SetChannelsAsync_Single(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
        {
            await Task.WhenAll(copters.Select(async copter =>
                await copter.SetChannelsAsync(ch2:ch2))).ConfigureAwait(false);

        }
       public async Task SetChannelsAsync_CMod(IEnumerable<ICopter> copters = null, ushort? ch1 = null, ushort? ch2 = null, ushort? ch3 = null, ushort? ch4 = null)
        {
            short copterId = 0;
            byte[] batchPacket = null;
            GetCopterIds(copters, out copterId, out batchPacket);
            
            byte[] packet = SetChannels(ch1, ch2, ch3, ch4);
            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
        }

        /// <summary>
        /// 开始写任务
        /// </summary>
        /// <param name="id">飞机ID</param>
        /// <param name="missions">任务列表</param>
        /// <returns></returns>
        /// 

        public async Task<bool> WriteMissionListAsync(ICopter copter, List<IMission> missions)
        {
            if (UseTransModule)
                return await WriteMissionListAsync_CMod(short.Parse(copter.Id), missions);
            else return await WriteMissionListAsync_Single(copter, missions);  

        }

        public async Task<bool> WriteMissionListAsync_Single(ICopter copter, List<IMission> missions)
        {
            return  await copter.WriteMissionListAsync(missions);
        }

        public async Task<bool> WriteMissionListAsync_CMod(short id, List<IMission> missions)
        {
            missions.Insert(0, PLCopter.PRE_TAKE_OFF_MISSION);
            List<MAVLink.mavlink_mission_item_t> mission_list = new List<MAVLink.mavlink_mission_item_t>();
            foreach (IMission mission in missions)
            {
                var frame = mission.Command == FlightCommand.Waypoint && mission.Sequence == 0 ? MAVLink.MAV_FRAME.GLOBAL : MAVLink.MAV_FRAME.GLOBAL_RELATIVE_ALT;

                var req = new MAVLink.mavlink_mission_item_t();

                req.target_system = 1;
                req.target_component = 1;

                req.command = (byte)mission.Command;

                req.current = 0;
                req.autocontinue = 1;

                req.frame = (byte)frame;
                if (mission.Command == FlightCommand.LEDControl)
                {
                    req.x = mission.R;
                    req.y = mission.G;
                    req.z = mission.B;
                }
                else
                {
                    req.x = (float)mission.Latitude;
                    req.y = (float)mission.Longitude;
                    req.z = mission.Altitude;
                }

                req.param1 = mission.Param1;
                req.param2 = mission.Param2;
                req.param3 = mission.Param3;
                req.param4 = mission.Param4;

                req.seq = mission.Sequence;
                mission_list.Add(req);
            }

            bool result =  await MissionPacket(id, (byte)MavComm.COMM_DOWNLOAD_MODE, mission_list.ToArray());
            
            lock(MissinLocker)
            {
                if (missionWriteState.ContainsKey(id))
                {
                    missionWriteState[id].StateReturn = MissionErrorId;
                    missionWriteState[id].ErrorCode = ErrorCode;
                    missionWriteState[id].SendAchieved = result;
                    missionWriteState[id].WriteSucceed = false;
                }
                else
                {
                    CommWriteMissinState state = new CommWriteMissinState(result);
                    state.StateReturn = MissionErrorId;
                    state.ErrorCode = ErrorCode;
                    missionWriteState.Add(id, state);
                }
                return result;
            }
        }

        /// <summary>
        /// 开始执行任务
        /// </summary>
        /// <param name="hour_utc">开始执行的小时</param>
        /// <param name="minute_utc">开始执行的分钟</param>
        /// <param name="second_utc">开始执行的秒</param>
        /// <param name="Missionlng">任务原点经度</param>
        /// <param name="Missionlat">任务原点纬度</param>
        /// <returns></returns>
        public async Task DoMissionStartAsync(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
            if (UseTransModule)
                await DoMissionStartAsync_CMod(copters,hour_utc, minute_utc, second_utc, Missionlng, Missionlat);
            else await DoMissionStartAsync_Single(copters, hour_utc, minute_utc, second_utc, Missionlng, Missionlat); 

        }
        public async Task DoMissionStartAsync_Single(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
           
           foreach (var vcopter in copters)
           {
               await vcopter.MissionStartAsync(hour_utc,
                  minute_utc,
                  second_utc,
                  Missionlng,
                  Missionlat
                  );
           }
          
        }


        public async Task DoMissionStartAsync_CMod(IEnumerable<ICopter> copters,int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {

            byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, (float)Missionlng, (float)Missionlat, 0, -1);
            short copterId = 0;
            byte[] batchPacket = null;
            //部分开始任务
            if (copters != null)
                GetCopterIds(copters, out copterId, out batchPacket);
            else
            {
                //只有全部飞机才广播
                if (Recomisopen)
                    await BroadcastSendAsync(packet);
            }
            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
        }


        /// <summary>
        /// 暂停任务
        /// </summary>
        public async Task DoMissionPauseAsync()
        {
            if (UseTransModule)
            {
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, 0, 0, 0, 0, 0, 1, 0);
                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                if (Recomisopen)
                    await BroadcastSendAsync(packet);
            }
            else
                Windows.Messages.Message.Show($"未开发完成！！");
        }

        /// <summary>
        /// 恢复任务
        /// </summary>
        public async Task DoMissionResumeAsync(int hour_utc, int minute_utc, int second_utc)
        {
            if (UseTransModule)
            {
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.MISSION_START, hour_utc, minute_utc, second_utc, 0, 0, 2, 0);
                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                if (Recomisopen)
                    await BroadcastSendAsync(packet);
            }
            else
                Windows.Messages.Message.Show($"未开发完成！！");
        }

        /// <summary>
        /// 解锁
        /// </summary>
        /// <param name="copters">要解锁的飞机</param>
        /// <returns></returns>
        public async Task UnlockAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);

                byte[] packet2 = SetModeAsync(FlightMode.ALT_HOLD);   //ALT_HOLD  LOITER 

                byte[] packet3 = DoARMAsync(true);

                byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
                if (Recomisopen)
                    await BroadcastSendAsync(data);

            }
            else
            {
                IEnumerable<ICopter> vcopters = copters;
                if (vcopters == null) vcopters = _allcopters;
                if (vcopters != null)
                {
                    foreach (var vcopter in vcopters)
                        await vcopter.UnlockAsync();
                }
            }
            //发送到广播模块--只针对全部飞机
            //if (enrecom&&(copters == null))
            {
               // if (enrecom)


            }
        }


        /// <summary>
        /// 抛物
        /// </summary>
        /// <param name="copters">要解锁的飞机</param>
        /// <returns></returns>
        public void ThrowoutAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                Task.Run(async () =>
                {
                    short copterId = 0;
                    byte[] batchPacket = null;
                    GetCopterIds(copters, out copterId, out batchPacket);

                    byte[] packet = DoThrowoutCommandAsync();
                    await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
                });
            }
            else
            {
               /*
                 Task.Run(async () =>
                {
                    foreach (var vcopter in copters)
                        await vcopter.LEDAsync();
                });
                */

            }
        }

        /// <summary>
        /// LED闪烁白灯
        /// </summary>
        /// <param name="copters">要闪烁的飞机</param>
        /// <returns></returns>
        public void LED_FlickerAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                Task.Run(async () =>
                {
                    short copterId = 0;
                    byte[] batchPacket = null;
                    GetCopterIds(copters, out copterId, out batchPacket);

                    byte[] packet = DoLEDCommandAsync(255,255,255);
                    await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
                    if (Recomisopen&&(copters == null))
                        await BroadcastSendAsync(packet);
                });
            }
            else
            {
                Task.Run(async () =>
                {
                    foreach (var vcopter in copters)
                        await vcopter.LEDAsync();
                });

            }

        }

        /*

        private int ledmode = 0;     //灯光模式 0常亮 1闪烁 2随机闪烁(RGB无意义)
        private float ledInterval = 0;     //间隔 单位秒 最小0.1
        private int ledtimes = 0;    //次数 (预留、暂取消其意义)
        private string ledRGB = "FFFFFF";      //灯光颜色

    */

        public void LED_TaskAsync( int  ledmode, float ledInterval, int ledtimes, string ledRGB, ICopter copter,bool oldver=true )
        {
            if (UseTransModule)
            {
                Task.Run(async () =>
                {
                    Color rightColor = (Color)ColorConverter.ConvertFromString("#" + ledRGB);
                    byte[] packet = DoTaskLEDCommandAsync(ledmode, ledInterval, ledtimes, (byte)rightColor.R, (byte)rightColor.G, (byte)rightColor.B);
                    await WriteCommPacketAsync(short.Parse(copter.Id), MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                });
            }
           

        }



        /// <summary>
        /// 设置参数
        /// </summary>
        /// <param name="paramname"></param>
        /// <param name="value"></param>
        /// <param name="copters"></param>
        /// <returns></returns>
        public async Task<int> SetParamAsync(string paramname, float value, IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = SetParam2Async(paramname, value);
                int packetNum = packet[2];
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
                if (Recomisopen && (copters == null))
                    await BroadcastSendAsync(packet);
                return packetNum;
            }
            else
            {
                IEnumerable<ICopter> vcopters = copters;
                if (vcopters == null) vcopters = _allcopters;
                if (vcopters != null)
                {
                    foreach (var vcopter in vcopters)
                        await vcopter.SetParamAsync(paramname, value);
                }
                return 0;
            }
        }

        /// <summary>
        /// 广播设置多个参数
        /// </summary>
        /// <param name="param"></param>
        /// <returns></returns>
        public async Task<int> SetMultipleParamAsync(params string[] param)
        {
            if (param.Length % 2 == 1)
                return 0;
            if (UseTransModule)
            {
  
                byte[] packet = null;
                int packetNum = 0;
                for (int i = 0; i < param.Length; i += 2)
                {
                    string paramname = param[i];
                    float value = float.Parse(param[i + 1]);
                    byte[] data = SetParam2Async(paramname, value);
                    packetNum = data[2];
                    packet = packet == null ? data : packet.Concat(data).ToArray();
                }
                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                return packetNum;
            }
            else
            {
                byte[] packet = null;
                int packetNum = 0;
                for (int i = 0; i < param.Length; i += 2)
                {
                    string paramname = param[i];
                    float value = float.Parse(param[i + 1]);
                   // foreach (var vcopter in Copters  )
                    //    await vcopter.SetParamAsync(paramname, value);

                }




                return 0;
            }
        }


        public void SetAllCoptersForWifi(IEnumerable<ICopter> copters )
        {

            _allcopters = copters;

        }
       


        /// <summary>
        /// 读取参数
        /// </summary>
        /// <param name="paramname"></param>
        /// <param name="copters"></param>
        /// <returns></returns>
        /// 


        public async Task ReadParamAsnyc(string paramname, IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = GetParam2Async(paramname);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                Windows.Messages.Message.Show($"未开发完成！！");
               
            }
        }

        /// <summary>
        /// 起飞测试
        /// </summary>
        /// <param name="alt">起飞高度</param>
        /// <param name="copters"></param>
        /// <returns></returns>
        public async Task TakeOffAsync(float alt, IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet1 = SetModeAsync(FlightMode.GUIDED);

                byte[] packet2 = SetChannels(1500, 1500, 1500, 1500);

                byte[] packet3 = DoCommandAsync(MAVLink.MAV_CMD.TAKEOFF, 0, 0, 0, 0, 0, 0, alt);

                byte[] data = packet1.Concat(packet2).Concat(packet3).ToArray();
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
            }
            else
            {
                foreach (var vcopter in copters)
                    await vcopter.TakeOffAsync();
               
            }
        }

        /// <summary>
        /// 返航
        /// </summary>
        /// <param name="copters"></param>
        /// <returns></returns>
        public async Task ReturnToLaunchAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = SetModeAsync(FlightMode.RTL);
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                foreach (var vcopter in copters)
                    await vcopter.ReturnToLaunchAsync ();

            }

        }




        /// <summary>
        /// 获取飞机版本
        /// </summary>
        /// <param name="copters"></param>
        /// <returns></returns>
        public async Task GetVersionsAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
                req.version = 0;
                byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {

                Windows.Messages.Message.Show($"未开发完成！！");
            }

        }

        public async Task  DoMissionEmergencyRetAsync(PLLocation takeoffcentloc,  PLLocation taskcentloc,  float mindistance,int rettime,bool descending)
        {
            Int32 param7 = rettime << 16;
            //低16位中的第一位表示是否直接降低高度
            if (descending)
                param7 = param7 | 1;
            if (UseTransModule)
            {
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.RETURN_TO_LAUNCH, 
                        (float)takeoffcentloc.Latitude  * 10000000,
                        (float)takeoffcentloc.Longitude * 10000000,//起飞高度不用传=0
                         mindistance*100, 0, 0,0,0);
                short copterId = 0;
                byte[] batchPacket = null;
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
                if (Recomisopen)
                    await BroadcastSendAsync(packet);
            }
            else
            {
                Windows.Messages.Message.Show($"未开发完成！！");
            }



        }


        public async Task GetCommsumAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                MAVLink.mavlink_sversion_request req = new MAVLink.mavlink_sversion_request();
                req.version = 1;
                byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST, req);
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {

                Windows.Messages.Message.Show($"未开发完成！！");
            }

        }


        
        /// <summary>
        /// 降落
        /// </summary>
        /// <returns></returns>
        public async Task LandAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = SetModeAsync(FlightMode.LAND);
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
                if (Recomisopen&&(copters == null))
                    await BroadcastSendAsync(packet);
            }
            else
            {
                IEnumerable<ICopter> vcopters = copters;
                if (vcopters == null) vcopters = _allcopters;
                if (vcopters != null)
                {
                    foreach (var vcopter in vcopters)
                        await vcopter.LandAsync();
                }

            }

        }

        /// <summary>
        /// 加锁
        /// </summary>
        /// <param name="armit"></param>
        /// <returns></returns>
        public async Task LockAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet1 = SetChannels(1500, 1500, 1100, 1500);
          
                byte[] packet2 = DoARMAsync(false);

                byte[] data = packet1.Concat(packet2).ToArray();
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
                if (Recomisopen&&(copters == null))
                    await BroadcastSendAsync(data);
            }
            else
            {
                IEnumerable<ICopter> vcopters = copters;
                if (vcopters == null) vcopters = _allcopters;
                if (vcopters != null)
                {
                    foreach (var vcopter in vcopters)
                        await vcopter.LockAsync();
                }
            }

        }

        /// <summary>
        /// 测试电机
        /// </summary>
        /// <param name="motor"></param>
        /// <returns></returns>
        public async Task MotorTestAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                //1-4代表1-4电机
                byte[] data1 = DoMotorTestAsync(1, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
                byte[] data2 = DoMotorTestAsync(2, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
                byte[] data3 = DoMotorTestAsync(3, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);
                byte[] data4 = DoMotorTestAsync(4, MAVLink.MOTOR_TEST_THROTTLE_TYPE.MOTOR_TEST_THROTTLE_PERCENT, 5, 5);

                byte[] data = data1.Concat(data2).Concat(data3).Concat(data4).ToArray();
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                    {
                        await vcopter.MotorTestAsync(1);
                        await vcopter.MotorTestAsync(2);
                        await vcopter.MotorTestAsync(3);
                        await vcopter.MotorTestAsync(4);
                    }

            }

        }

        /// <summary>
        /// 切换悬停模式
        /// </summary>
        /// <returns></returns>
        public async Task HoverAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet1 = SetChannels(1500, 1500, 1500, 1500);

                byte[] packet2 = SetModeAsync(FlightMode.LOITER);

                byte[] data = packet1.Concat(packet2).ToArray();
                await WriteCommPacketAsync((short)copterId, MavComm.COMM_DOWNLOAD_COMM, data, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                    await vcopter.HoverAsync();

            }

        }

        /// <summary>
        /// 切换手动模式
        /// </summary>
        /// <param name="copters"></param>
        /// <returns></returns>
        public async Task FloatAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = SetModeAsync(FlightMode.ALT_HOLD);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters!=null)
                foreach (var vcopter in copters)
                    await vcopter.FloatAsync();

            }
        }

        /// <summary>
        /// 校准加速计
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoStartPreflightCompassAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 0, 0, 0, 0, 1, 0, 0);

            }
        }

        /// <summary>
        /// 校准加速计下一步
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoNextPreflightCompassAsync(IEnumerable<ICopter> copters = null)
        {
            short copterId = 0;
            byte[] batchPacket = null;
            GetCopterIds(copters, out copterId, out batchPacket);

            MAVLink.mavlink_command_ack_t req = new MAVLink.mavlink_command_ack_t();
            req.command = 1;
            req.result = 1;

            if (UseTransModule)
            {
                byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_COMMAND_ACK, req);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                    {
                       await vcopter.DoCommandAckAsync(1, 1);
                    }
            }
        }

        /// <summary>
        /// 重启飞控
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoRestartFCAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);
                //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1,1, 0, 0, 0, 0);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_REBOOT_SHUTDOWN, 1, 1, 1, 0, 0, 0, 0);

            }
        }

        /// <summary>
        /// 校准陀螺仪
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoCalibrationPreflightAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);
                //只校陀螺仪，详细说明见;https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_CALIBRATION
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0);

            }
        }
        /// <summary>
        /// 校准指南针
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoCalibrationCompassAsync(IEnumerable<ICopter> copters = null)
        {
            if (UseTransModule)
            {
                short copterId = 0;
                byte[] batchPacket = null;
                GetCopterIds(copters, out copterId, out batchPacket);

                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet, batchPacket).ConfigureAwait(false);
            }
            else
            {
                if (copters != null)
                    foreach (var vcopter in copters)
                        await vcopter.DoCommandAsync((int)MAVLink.MAV_CMD.DO_START_MAG_CAL, 0, 1, 1, 0, 0, 0, 0);

            }
        }

        /// <summary>
        /// 放弃校准指南针
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoCancelCalibrationCompassAsync(short copterId)
        {
            if (UseTransModule)
            {
                byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_CANCEL_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
                await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
            }
            else
            {
                Windows.Messages.Message.Show($"未开发完成！！");
            }
        }
        /// <summary>
        /// 确认校准磁罗盘
        /// </summary>
        /// <param name="copterId"></param>
        /// <returns></returns>
        public async Task DoAcceptCalibrationCompassAsync(short copterId)
        {
            byte[] packet = DoCommandAsync(MAVLink.MAV_CMD.DO_ACCEPT_MAG_CAL, 0, 0, 1, 0, 0, 0, 0);
            await WriteCommPacketAsync(copterId, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
        }


        byte rtcm_tmpser = 0;  //用于通讯模块的
        byte rtcm_Broadser = 0;//用于广播的

        /// <summary>
        /// 发送到广播端口
        /// </summary>
        /// <param name="packet">发送数据</param>
        /// <param name="reopensend">发送失败是否要重新打开再发一次</param>
        /// <returns></returns>
        public async Task BroadcastSendAsync(byte[] packet,bool reopensend = false)
        {
            
            bool sendok = false;
            //并且没有最后打开的端口说明已经人为关闭了
            if (last_reserialport == "")
            {
                return;
            }
            try
            {
                //防止阻塞，异步发送
                await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
                //RecomPort.Write(packet, 0, packet.Length);
                sendok = true;
            }
            catch (Exception ex)
            {
                Windows.Messages.Message.Show("广播端口发送失败...");
            }

            if (!sendok&& reopensend)
            {
                //再次打开串口
                try
                {
                    ReOpenRtcmserial();
                    if (Recomisopen) Windows.Messages.Message.Show("广播端口打开成功!");
                }
                catch (Exception ex)
                {
                    Windows.Messages.Message.Show("再次打开串口失败" + ex.Message);
                    return;
                }
                //再次发送
                if (Recomisopen)
                {
                    try
                    {
                        await RecomPort.BaseStream.WriteAsync(packet, 0, packet.Length);
                        //RecomPort.Write(packet, 0, packet.Length);
                    }
                    catch (Exception ex)
                    {
                        Windows.Messages.Message.Show("再次发送失败...");
                    }
                }

            }
        }

        public async Task BroadcastbackupGpsDataAsync(byte[] packet)
        {
            await BroadcastSendAsync(packet,true);
        }
 
        // get sum crc 计算数据校验和
        public byte checkrtrcmsum(byte[] data, ushort length)
        {
            byte rtcm_sumcrc = 0;
            for ( int i=0; i< length; i++)
            {
                rtcm_sumcrc += data[i];
            }
            return rtcm_sumcrc; 
        }


        /// <summary>
        ///广播Rtk               //-------------使用中的RTK广播函数------------
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <returns></returns>
        public async Task InjectGpsDataAsync(byte[] data, ushort length,bool enrecom)
        {
            //由于通讯模块限制一次只能发110个字节
            if (UseTransModule)
            {
                MAVLink.mavlink_gps_inject_data_t gps = new MAVLink.mavlink_gps_inject_data_t();
                var msglen = 110;
                int datalen = 0;
                var len = (length % msglen) == 0 ? length / msglen : (length / msglen) + 1;
                for (int a = 0; a < len; a++)
                {
                    datalen = Math.Min(length - a * msglen, msglen);
                    gps.data = new byte[msglen];
                    Array.Copy(data, a * msglen, gps.data, 0, datalen);
                    //gps.data[0] = rtcm_tmpser++;
                    gps.len = (byte)datalen;
                    gps.target_component = rtcm_tmpser++;
                    //实测一旦收到数据都是正确的，张伟已经做过crc检验了，为了兼容性不再做了
                    //如果需要做，飞控里面需要把注释的代码放开，并且不再兼容之前的固件
                    gps.target_system = checkrtrcmsum(gps.data, (ushort)datalen);  //默认:1

                    //   Console.WriteLine("                                              ["+ gps.target_component + "]总长: "+ length +" 发送第 "+ (a+1) + " 个包,长度: " + (ushort)datalen );

                    byte[] packet = GenerateRTKPacketAsync(MAVLink.MAVLINK_MSG_ID_GPS_INJECT_DATA, gps, gps.len);

                    //发送到通讯模块
                    await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                    //发送到广播端口作为备用数据源
                    if (enrecom)
                        await BroadcastbackupGpsDataAsync(packet);
                    await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到

                    // 重发一次，有序列号(target_component)飞机可以检测出来重复接收的
                    //需要新固件支持
                    await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                    //发送到广播端口作为备用数据源
                    if (enrecom)
                        await BroadcastbackupGpsDataAsync(packet);

                    await Task.Delay(80).ConfigureAwait(false); //没有延时得话如果超过110长度，连续发可能收不到
                }
            }
            else
            {
                if (_allcopters != null)
                    foreach (var vcopter in _allcopters)
                        await vcopter.InjectGpsDataAsync(data, length);
            }
        }

        int inject_seq_no = 0;

        /// <summary>
        /// 插入RTK数据
        /// </summary>
        /// <param name="data"></param>
        /// <param name="length"></param>
        /// <returns></returns>
        public async Task InjectGpsRTCMDataAsync(byte[] data, int length)
        {
            MAVLink.mavlink_gps_rtcm_data_t gps = new MAVLink.mavlink_gps_rtcm_data_t();
            var msglen = 180;  //飞机端是180，之前也是180和通讯盒可能也有关系

            //             if (length > msglen * 4)
            //                 log.Error("Message too large " + length);

            // number of packets we need, including a termination packet if needed
            var nopackets = (length % msglen) == 0 ? length / msglen + 1 : (length / msglen) + 1;

           // if (nopackets>1)
               // Plane.Windows.Messages.Message.Show("   RTCM_Mut:  " + length);

            if (nopackets >= 4)
                nopackets = 4;

            // flags = isfrag(1)/frag(2)/seq(5)

            for (int a = 0; a < nopackets; a++)
            {
                // check if its a fragment
                if (nopackets > 1)
                    gps.flags = 1;
                else
                    gps.flags = 0;

                // add fragment number
                gps.flags += (byte)((a & 0x3) << 1);

                // add seq number
                gps.flags += (byte)((inject_seq_no & 0x1f) << 3);

                // create the empty buffer
                gps.data = new byte[msglen];

                // calc how much data we are copying
                int copy = Math.Min(length - a * msglen, msglen);

                // copy the data
                Array.Copy(data, a * msglen, gps.data, 0, copy);

                // set the length
                gps.len = (byte)copy;

                byte[] packet = GeneratePacket(MAVLink.MAVLINK_MSG_ID_GPS_RTCM_DATA, gps);

                // packet[2]
                //Plane.Windows.Messages.Message.Show("["+ packet[2]+"]RTCM:"+ gps.len);

               // Console.WriteLine("RTCM:" + gps.len+ "[" + data[0]+","+ data[1] + "," + data[2] + "," + data[3] + "," + data[4]+"]");

                //Windows.Messages.Message.Show($"{DateTime.Now.ToString("HH:mm:ss:fff")} 单次长度 = {packet.Length}");
                //老版本在这发送，新版本在StartRtcmLoop里面一次性打包4个180字节发送了
                await WriteCommPacketAsync(0, MavComm.COMM_DOWNLOAD_COMM, packet).ConfigureAwait(false);
                /*  //长RTCP数据发送还没开发完
                lock (lock_rtcm)
                {
                    rtcm_packets.Add(packet);
                    if (rtcm_packets.Count == 1)
                    {
                        waitRtcmTime = DateTime.Now;
                        starttime = true;
                    }
                }
                */

            }
        }



        /// <summary>
        /// 开始循环检测Rtcm数量
        /// </summary>
        /// <returns></returns>
        public async Task StartRtcmLoop()
        {
            rtcm_run = true;
            waitRtcmTime = DateTime.Now;
            await Task.Factory.StartNew(RtcmLoop);
        }

        /// <summary>
        /// 结束循环检测Rtcm数量
        /// </summary>
        /// <returns></returns>
        public async Task CloseRtcmLoop()
        {
            rtcm_run = false;
            await Task.Delay(1);
        }

        public string last_reserialport ="";
        public void ReOpenRtcmserial()
        {
            CloseResendRtcmserial();
            OpenResendRtcmserial(last_reserialport);

        }

        public void CloseResendRtcmserial(bool cleanport=false)
        {
            if (Recomisopen)
            {
                RecomPort.Close();
                RecomPort.Dispose();
            }
            Recomisopen = false;
            //清除端口
            if (cleanport)
                last_reserialport="";

        }

        public string BoardPortStatusStr
        {
            get { 
                if (Recomisopen)
                    return "广播端口已打开"; 
                else
                    return "广播端口未打开";
            }
            
        }



        public bool OpenResendRtcmserial(string reserialport)
        {
            RecomPort = new SerialPort(reserialport);
            RecomPort.BaudRate = 57600;
            RecomPort.Parity = Parity.None;
            RecomPort.StopBits = StopBits.One;
            RecomPort.DataBits = 8;
            RecomPort.Handshake = Handshake.None;
            RecomPort.WriteTimeout = 500; // 设置写入超时为500毫秒,防止Close时候卡死


            try
            {
                RecomPort.Open();
                Recomisopen = true;
                last_reserialport = reserialport;
            }
            catch (Exception ex)
            {
              //  Alert.Show("转发端口打开失败" + ex.Message);
            }
 
            return Recomisopen;
        }




        /// <summary>
        /// 产生RTK包
        /// </summary>
        /// <typeparam name="TMavlinkPacket"></typeparam>
        /// <param name="messageType"></param>
        /// <param name="indata"></param>
        /// <param name="rtklength"></param>
        /// <returns></returns>
        public byte[] GenerateRTKPacketAsync<TMavlinkPacket>(byte messageType, TMavlinkPacket indata, int rtklength)
        {
            byte[] data;
            int rtkdataleng = rtklength + 3;

            data = MavlinkUtil.StructureToByteArray(indata);


            byte[] packet = new byte[rtkdataleng + 6 + 2];

            packet[0] = MAVLink.MAVLINK_STX;
            packet[1] = (byte)(rtkdataleng);
            packet[2] = (byte)packetcount;
            packetcount++;
            packet[3] = 255; // this is always 255 - MYGCS
            packet[4] = (byte)MAVLink.MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER;
            packet[5] = messageType;

            int i = 6;
            foreach (byte b in data)
            {
                if (i < rtkdataleng + 6)
                    packet[i] = b;
                else break;
                i++;
            }

            ushort checksum = MavlinkCRC.crc_calculate(packet, packet[1] + 6);

            checksum = MavlinkCRC.crc_accumulate(MAVLink.MAVLINK_MESSAGE_CRCS[messageType], checksum);

            byte ck_a = (byte)(checksum & 0xFF); ///< High byte
            byte ck_b = (byte)(checksum >> 8); ///< Low byte

            packet[i] = ck_a;
            i += 1;
            packet[i] = ck_b;
            i += 1;

            return packet;
        }

   
    }

}