using Plane.Communication;
using Plane.Geography;
using System;
using System.Diagnostics;
using System.Drawing;
using System.Threading;
using System.Threading.Tasks;
using static Plane.Copters.Constants;
using System.Windows.Media;
using Plane.CopterControllers;

namespace Plane.Copters
{
    /// <summary>
    /// 虚拟的 <see cref="ICopter"/> 实现。
    /// </summary>
    [DebuggerDisplay("Name={Name}")]
    public partial class FakeCopter : CopterImplSharedPart, IFakeCopter
    {
        /// <summary>
        /// 心跳间隔，单位为毫秒。
        /// </summary>
        private const int HEARTBEAT_INTERVAL = 200;
        /// <summary>
        /// 更新虚拟飞行器状态的时间间隔，单位为毫秒。
        /// </summary>
        static private int UPDATE_INTERVAL = 100;  //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 

        static private int UPDATE_INTERVAL_TEMP = 50;
        /// <summary>
        /// 在一个更新间隔中的最大移动距离。
        /// </summary>
        private float MAX_MOVE_IN_INTERVAL = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;

        /// <summary>
        /// 高速模式下，在一个更新间隔中的最大移动距离。
        /// </summary>
        private float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4;  //MAX_MOVE_IN_INTERVAL*4



        // private int _UPDATE_INTERVAL = 50;

        /// <summary>
        /// 对飞行器的模拟是否正在运行。
        /// </summary>
        private bool _isRunning = false;

        /// <summary>
        /// 上次计算速度时的位置。
        /// </summary>
        private ILocation _lastCalcSpeedPoint;

        /// <summary>
        /// 上次计算速度的时间。
        /// </summary>
        private DateTime _lastCalcSpeedTime;

        /// <summary>
        /// 高速模式下，按比例缩放过的在一个更新间隔中的最大移动距离。
        /// </summary>
        private float _scaledFastMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000 * 4;  //MAX_MOVE_IN_INTERVAL_FAST

        /// <summary>
        /// 按比例缩放过的在一个更新间隔中的最大移动距离。
        /// </summary>
        private float _scaledMaxMoveInInterval = MAX_VEL * UPDATE_INTERVAL_TEMP / 1000;  //MAX_MOVE_IN_INTERVAL

        /// <summary>
        /// 速度缩放比例。
        /// </summary>
        private float _speedScale = 1;

        /// <summary>
        /// 自动起飞的目标高度。
        /// </summary>
        private int _takeOffTargetAltitude;

        /// <summary>
        /// FlyTo 的目标高度。
        /// </summary>
        private float _targetAlt;
        //航点开始高度
        private float _startAlt;

        private float _Lng_delta;
        private float _Lat_delta;

        private float _flytime;
        private DateTime _startTicks;

        private float _distance_xy;
        private float _distance_z;
        private float _xy_speed;
        private float _z_speed;
        private int _flytype;
        private float currflytime;
        // 根据飞行时间计算飞行距离。
        private float currdis_xy;
        private float currdis_z;


        // 目标点相对于开始位置的方向。
        private double _direction;

        /// <summary>
        /// FlyTo 的目标纬度。
        /// </summary>
        private double _targetLat;

        /// <summary>
        /// FlyTo 的目标经度。
        /// </summary>
        private double _targetLng;
        /// <summary>
        /// FlyTo 的目标纬度。
        /// </summary>
        private double _startLat;

        /// <summary>
        /// FlyTo 的目标经度。
        /// </summary>
        private double _startLng;


        /// <summary>
        /// FlyTo 的目标经度。
        /// </summary>
        private bool _ShowLED;

        private System.Drawing.ColorConverter rgbconverter;


        private PLLocation _takeoffcentloc;
        private PLLocation _taskcentloc;
        private float _mindistance;
        private int _rettime;
        private bool _descending;
        int Emergency_Retstatus = 0;
        //返航路径第一个航点
        PLLocation Emergency_firstloc;

        /// <summary>
        /// 使用 <see cref="SynchronizationContext.Current"/> 创建 <see cref="FakeCopter"/> 实例。
        /// </summary>
        public FakeCopter() : this(SynchronizationContext.Current)
        {


        }


        new public int sim_update_int
        {
            get { return UPDATE_INTERVAL; }
            set
            {
                //最大移动距离
                MAX_MOVE_IN_INTERVAL = MAX_VEL * value / 1000;
                //快速模式最大移动距离
                MAX_MOVE_IN_INTERVAL_FAST = MAX_MOVE_IN_INTERVAL * 4;
                //速度缩放后快速速度距离
                _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;
                //速度缩放后速度距离
                _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
                Set(nameof(sim_update_int), ref UPDATE_INTERVAL, value);
            }
        }


        public Task DoCommandAckAsync(ushort command, byte result)
        {
            return TaskUtils.CompletedTask;
        }

        /// <summary>
        /// 创建 <see cref="FakeCopter"/> 实例。
        /// </summary>
        /// <param name="uiSyncContext">UI 线程的同步上下文。</param>
        /// <param name="takeOffTargetAltitude">自动起飞时的目标高度。</param>
        /// <param name="speedScale">速度缩放比例。</param>
        public FakeCopter(SynchronizationContext uiSyncContext, int takeOffTargetAltitude = 10, float speedScale = 1, IConnection connection = null)
            : base(uiSyncContext)
        {
            Latitude = 23.155382266268134;
            Longitude = 113.45038586296141;

            Elevation = 20;
            this.PropertyChanged += (sender, e) =>
            {
                switch (e.PropertyName)
                {
                    case nameof(Altitude):
                        Elevation = Altitude + 20;
                        break;

                    case nameof(IsUnlocked):
                        if (IsUnlocked)
                        {
                            // 王海, 20151029, 在解锁时上次计算速度点、上次计算速度时刻。
                            _lastCalcSpeedPoint = new PLLocation { Latitude = Latitude, Longitude = Longitude, Altitude = Altitude };
                            _lastCalcSpeedTime = DateTime.Now;
                        }
                        break;
                }
            };

            _takeOffTargetAltitude = takeOffTargetAltitude;

            _speedScale = speedScale;
            _scaledMaxMoveInInterval = MAX_MOVE_IN_INTERVAL * _speedScale;
            _scaledFastMaxMoveInInterval = MAX_MOVE_IN_INTERVAL_FAST * _speedScale;

            this.Connection = connection ?? EmptyConnection.Instance;
            _isRunning = true;
            // 持续计算并更新虚拟飞行器的状态。
            // Task.Run(async () =>

            Task.Factory.StartNew(async () =>

            {
                while (_isRunning)
                {
                    Update();
                    await TaskUtils.Delay(UPDATE_INTERVAL).ConfigureAwait(false);
                }
            }
            , TaskCreationOptions.LongRunning
            );


            ++HeartbeatCount;
            GpsFixType = GpsFixType.Fix3D;
            GpsHdop = 1;
            IsGpsAccurate = true;
            HasSwitchedToGpsMode = true;
            GC_xy = new GuidController();
            GC_z = new GuidController();

            /*
            // 持续假装收到飞行器发来的心跳。
            Task.Run(async () =>
            {
                while (true)
                {
                    if (_isRunning)
                    {
                        ++HeartbeatCount;

                        IsCheckingConnection = false;

                        if (HeartbeatCount >= 10)
                        {
                            // 收到若干个心跳之后，设置一下 GPS 相关属性。
                            GpsFixType = GpsFixType.Fix3D;
                            GpsHdop = 1;
                            IsGpsAccurate = true;
                            HasSwitchedToGpsMode = true;
                        }

                        _uiSyncContext.Post(() => RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(HeartbeatCount)));
                    }
                    await TaskUtils.Delay(HEARTBEAT_INTERVAL).ConfigureAwait(false);
                }
            });
            */
        }

        public IConnection Connection { get; set; }

        public string Id { get; set; }

        private int ReturnToLaunchAltitude { get { return _takeOffTargetAltitude; } }
        private GuidController GC_xy;
        private GuidController GC_z;


        public Task ConnectAsync()
        {
            IsConnected = true;
            _isRunning = true;
            IsCheckingConnection = true;
            return TaskUtils.CompletedTask;
        }

        public Task DisconnectAsync()
        {
            IsConnected = false;
            _isRunning = false;
            IsCheckingConnection = false;
            return TaskUtils.CompletedTask;
        }
        private void BuildPath()
        {
            float taralt;
            //返航高度参数
            float retalt = 15;

            if (_descending)
            {
                if (Altitude <= retalt) taralt = Altitude;
                else
                {
                    float maxalt = Altitude - retalt;
                    //降低一个随机高度，随机高度介于0和maxalt之间，也就是目标高度不要低于retalt, 防止飞机在同一高度飞行
                    taralt = (float)(Altitude - (new Random().NextDouble() * maxalt));
                }
                Emergency_firstloc = new PLLocation(this.Latitude,this.Longitude, taralt);
            }
            else
            {
               //计算当前位置和起飞点的距离
                float dis = (float)this.CalcDistance(TakeOffPoint.Latitude, TakeOffPoint.Longitude,Altitude);
                //随机水平飞行距离
                float maxdis = (float)new Random().NextDouble() * dis;




            }

           
        }
        public Task EmergencyRetAsync(PLLocation takeoffcentloc, PLLocation taskcentloc, float mindistance, int rettime, bool descending)
        {
            _takeoffcentloc = takeoffcentloc;
            _taskcentloc = taskcentloc;
            _mindistance = mindistance;
            _rettime = rettime;
            _descending = descending;
            Emergency_Retstatus = 0;
            //计算返航路径
            BuildPath();
            Mode = FlightMode.EMERG_RTL;
            return TaskUtils.CompletedTask;
        }

        protected override Task FlyToCoreAsync(double lat, double lng, float alt, float flytime = 0, int flytype = 0)
        {
            if (!IsEmergencyHoverActive)
            {
                _targetLat = lat;
                _targetLng = lng;
                _targetAlt = alt;
                _startLat = Latitude;
                _startLng = Longitude ;
                _startAlt = Altitude;
                _direction = this.CalcDirection2D(lat, lng);

                _flytime = flytime*1000; //ms
                _startTicks = DateTime.Now; //ms


                _Lng_delta = (float)( Math.Sin(_direction) * GeographyUtils.CalcMetersToLngSpan(Latitude));
                _Lat_delta =(float)( Math.Cos(_direction) * GeographyUtils.METERS_TO_LAT_SPAN);
               
                //计算xy和x方向距离
                _distance_xy = (float)this.CalcDistance(lat, lng, Altitude);
                _distance_z = alt - Altitude;
                Console.WriteLine($"{this.Id}d:{_distance_xy},lat{this.Latitude},lng{this.Longitude },tlat:{lat}tlng:{lng}");

                _flytype = flytype;
                // _xy_speed = _distance_xy / _flytime;
                // _z_speed = _distance_z / _flytime;
                GC_xy.initgetspeeddist(_flytype, _flytime / 1000, _distance_xy, acc_xy, maxspeed_xy);
                if (_targetAlt>Altitude )
                    GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_up);
                else
                    GC_z.initgetspeeddist(_flytype, _flytime / 1000, _distance_z, acc_z, maxspeed_down);



                Mode = FlightMode.GUIDED;


            }
            return TaskUtils.CompletedTask;
        }

        public Task GetCopterDataAsync()
        {
            return TaskUtils.CompletedTask;
        }

        public Task<float> GetParamAsync(string paramName, int millisecondsTimeout = Timeout.Infinite)
        {
            // TODO: 王海, 20150806, 实现仿真的 GetParamAsync。
            return Task.FromResult(0f);
        }

        public Task LockAsync()
        {
            if (IsUnlocked)
            {
                IsUnlocked = false;
            }
            return TaskUtils.CompletedTask;
        }

        public Task LEDAsync()
        {
            return TaskUtils.CompletedTask;
        }

        public Task MotorTestAsync(int motor)
        {
            return TaskUtils.CompletedTask;
        }

        public override Task SetChannelsAsync()
        {
            Channel1 = DesiredChannel1 ?? Channel1;
            Channel2 = DesiredChannel2 ?? Channel2;
            Channel3 = DesiredChannel3 ?? Channel3;
            Channel4 = DesiredChannel4 ?? Channel4;
            Channel5 = DesiredChannel5 ?? Channel5;
            Channel6 = DesiredChannel6 ?? Channel6;
            Channel7 = DesiredChannel7 ?? Channel7;
            Channel8 = DesiredChannel8 ?? Channel8;
            return TaskUtils.CompletedTask;
        }

        public override Task SetMobileControlAsync()
        {
            Channel1 = DesiredChannel1 ?? Channel1;
            Channel2 = DesiredChannel2 ?? Channel2;
            Channel3 = DesiredChannel3 ?? Channel3;
            Channel4 = DesiredChannel4 ?? Channel4;
            Channel5 = DesiredChannel5 ?? Channel5;
            Channel6 = DesiredChannel6 ?? Channel6;
            Channel7 = DesiredChannel7 ?? Channel7;
            Channel8 = DesiredChannel8 ?? Channel8;
            Yaw = DesiredYaw ?? Yaw;
            Heading = (short)Yaw;
            return TaskUtils.CompletedTask;
        }

        //得到收到的总数据量
        public void GetCommunicationNumber(out int recnumber, out int sendnumber)
        {
            recnumber = 0;
            sendnumber = 0;
        }


        //重设数据量
        public void ResetCommunicationNumber()
        {

        }

        public async Task DoCommandAsync(int actionid, float p1, float p2, float p3, float p4, float p5, float p6, float p7)
        {

            await Task.Delay(50).ConfigureAwait(false);
           

        }
        public Task SetParamAsync(string paramName, float value, int millisecondsTimeout = Timeout.Infinite)
        {
            // TODO: 王海, 20150807, 实现仿真的 SetParamAsync。
            return TaskUtils.CompletedTask;
        }
        public bool GetShowLEDAsync()
        {
            return _ShowLED;
        }             
        public Task SetShowLEDAsync(bool Ledon)
        {
            _ShowLED = Ledon;
            RaiseLocationChanged();
            return TaskUtils.CompletedTask;
        }


        public Task SetShowRGBLEDAsync(int RedValue, int GreenValue, int BlueValue)
        {
            return TaskUtils.CompletedTask;
        }


        public Task SetShowLEDFlashAsync(int Flashtype, int millisecondsTime)
        {
            bool Ledison;
            Ledison = GetShowLEDAsync();
            if (!Ledison)
            {
                 SetShowLEDAsync(true);
                 Task.Delay(millisecondsTime).ConfigureAwait(false);
                 SetShowLEDAsync(false);
            }
            else
            {
                 SetShowLEDAsync(false);
                 Task.Delay(millisecondsTime).ConfigureAwait(false);
                 SetShowLEDAsync(true);
            }
           
            return TaskUtils.CompletedTask;
        }

        public void SetProperties(
            string id = null,   //"Junqing's Drone",
            double? latitude = null,   //23.14973333,
            double? longitude = null,   //113.40974166,
            float? altitude = null,   //0,
            string name = null,   //"王海的飞行器",
            byte? batteryPer = null,   //10,
            short? heading = null,   //33,
            bool? isConnected = null,   //true,
            float? pitch = null,   //-70,
            float? roll = null,   //28,
            byte? satCount = null,   //6,
            float? airSpeed = null,   //3.333,
            double? flightDistance = null,   //100.388,
            double? flightDistance2D = null, // 100.88,
            TimeSpan? flightTimeSpan = null)
        {
            if (id != null) Id = id;
            if (name != null) Name = name;
            if (latitude != null) Latitude = latitude.Value;
            if (longitude != null) Longitude = longitude.Value;
            if (altitude != null) Altitude = altitude.Value;
            if (batteryPer != null) BatteryPer = batteryPer.Value;
            if (heading != null) Heading = heading.Value;
            if (isConnected != null) IsConnected = isConnected.Value;
            if (pitch != null) Pitch = pitch.Value;
            if (roll != null) Roll = roll.Value;
            if (satCount != null) SatCount = satCount.Value;
            if (airSpeed != null) AirSpeed = airSpeed.Value;
            if (flightDistance != null) FlightDistance = flightDistance.Value;
            if (flightDistance2D != null) FlightDistance2D = flightDistance2D.Value;
            if (flightTimeSpan != null) FlightTimeSpan = flightTimeSpan.Value;
        }

        public Task StartPairingAsync()
        {
            RaisePairingCompleted(new PairingCompletedEventArgs(true, 0, 0));
            return TaskUtils.CompletedTask;
        }

        public Task StopPairingAsync()
        {
            return TaskUtils.CompletedTask;
        }

        public async Task TakeOffAsync(float alt)
        {
            await UnlockAsync().ConfigureAwait(false);
            _takeOffTargetAltitude = (int)alt;
            await TakeOffAsync().ConfigureAwait(false);
        }
        DateTime MissionStartTime;
        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
            MissionStartTime = DateTime.Now;
            TakeOffPoint = new PLLocation(Missionlat, Missionlng, 0);

            return TaskUtils.CompletedTask;
        }


        public async Task UnlockAsync()
        {
            if (!IsUnlocked)
            {
                await SetChannelsAsync(
                    ch1: 1500,
                    ch2: 1500,
                    ch3: 1100,
                    ch4: 1500
                ).ConfigureAwait(false);
                if (await SetModeAsync(FlightMode.ALT_HOLD).ConfigureAwait(false))
                {
                    IsUnlocked = true;
                }
            }
        }

        /// <summary>
        /// 无论在不在紧急悬停状态下都可用的切换模式方法。
        /// </summary>
        /// <param name="mode">模式。</param>
        /// <param name="millisecondsTimeout">超时。</param>
        /// <returns>成功与否。</returns>
        internal override Task<bool> SetModeCoreAsync(FlightMode mode, int millisecondsTimeout = 5000)
        {
            Mode = mode;
            return Task.FromResult(true);
        }

        protected override void UpdateFlightDataIfNeeded()
        {
            //计算飞机距离，没必要
            /*
            if (!TakeOffPoint.IsNullOrEmpty())
            {
                FlightDistance = TakeOffPoint.CalcDistance(this);
                FlightDistance2D = TakeOffPoint.CalcDistance2D(this);
            }
           

            if (FlightTimeSpan.TotalSeconds > 0)
            {
                if (_lastCalcSpeedPoint.IsNullOrEmpty())
                {
                    if (!this.IsNullOrEmpty())
                    {
                        _lastCalcSpeedPoint = new PLLocation(this);
                        _lastCalcSpeedTime = DateTime.Now;
                    }
                }
                else
                {
                    var movedDistance = _lastCalcSpeedPoint.CalcDistance(this);
                    var movedTime = DateTime.Now - _lastCalcSpeedTime;
                    if (movedDistance >= 3 || movedTime.TotalSeconds >= 3)
                    {
                        var airSpeed = movedDistance / movedTime.TotalSeconds;
                        if (airSpeed < 100)  // 王海, 20151023, 速度过大时不正常，经纬度可能有错误。
                        {
                            AirSpeed = (float)airSpeed;
                        }
                        //Windows.Messages.Message.Show(AirSpeed.ToString());
                        GroundSpeed = AirSpeed;
                        _lastCalcSpeedPoint = new PLLocation(this);
                        _lastCalcSpeedTime = DateTime.Now;
                    }
                }
            }
            */
        }

        private void MoveToPointImmediately(double lat, double lng, float alt)
        {
            Latitude = lat;
            Longitude = lng;
            Altitude = alt;
            Roll = 0;
            Pitch = 0;
        }
        public Task InjectGpsDataAsync(byte[] data, ushort length)
        {
            return TaskUtils.CompletedTask;

        }

        public static System.Drawing.Color GetRandomColor()
        {
            Random rand = new Random();
            return System.Drawing.Color.FromArgb(rand.Next(256), rand.Next(256), rand.Next(256));
        }

        private DateTime led_laston;
        private System.Drawing.Color Led_color;

        //更新显示颜色，根据设置的led参数
        private  void   UpdateShowColor()
        {
            // 使用实例化的对象调用ConvertFromString
            //LEDShowColor = (System.Drawing.Color)converter.ConvertFromString("#" + LEDColor);
            // 创建ColorConverter实例用于颜色转换
            if ( rgbconverter==null) rgbconverter=new System.Drawing.ColorConverter();
            //简化版的颜色模拟
            switch (LEDMode)
            {
                case 0:
                        if (LEDColor != null)
                        LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
                        else LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#FFFFFF");

                    break;
                //闪烁
                case 1:
                case 2:
                case 3:
                case 4:
                case 5:
                case 6:
                case 7:
                case 8:
                case 9:
                case 10:
                case 11:
                case 12:
                case 13:
                case 14:

                    if (DateTime.Now.AddMilliseconds(-(LEDInterval*1000)) >= led_laston)
                    {
                        led_laston = DateTime.Now;
                        if (LEDShowColor != System.Drawing.Color.Black)
                        {
                            LEDShowColor = System.Drawing.Color.Black;

                        }
                        else
                        {
                            if ((LEDMode == 1) || (LEDMode == 4) || (LEDMode == 5) || (LEDMode == 6) || (LEDMode == 7) || (LEDMode == 9) || (LEDMode == 11) || (LEDMode == 13))
                                LEDShowColor = (System.Drawing.Color)rgbconverter.ConvertFromString("#" + LEDColor);
                            if ((LEDMode == 2)||(LEDMode == 3) || (LEDMode == 8) || (LEDMode == 10) || (LEDMode == 12) || (LEDMode == 14))
                                 LEDShowColor = GetRandomColor();
                        }
                    }

                    break;
              
            }





        }

        private bool ReachedDestination(double destLat, double destLng, float destAlt) => this.CalcDistance(destLat, destLng, destAlt) < 0.1;

        /// <summary>
        /// 计算并更新虚拟飞行器状态。
        /// </summary>
        private void Update()
        {
            if (IsUnlocked)
            {
                // 飞行器接触地面（只判断 Altitude <= 0）后，如果处于 LAND 模式或者保持 Channel3 < MIN_HOVER_CHANNEL，在一段时间后自动锁定。
                if ((Mode == FlightMode.LAND || Channel3 < MIN_HOVER_CHANNEL) && Altitude <= 0)
                {
                    Task.Run(async () =>
                    {
                        for (int i = 0; i < 4000 / 20; i++)
                        {
                            await TaskUtils.Delay(20).ConfigureAwait(false);
                            if ((Mode != FlightMode.LAND && Channel3 >= MIN_HOVER_CHANNEL) || !IsUnlocked)
                            {
                                return;
                            }
                        }
                        IsUnlocked = false;
                    });
                }
                else
                {
                    switch (Mode)
                    {
                        case FlightMode.ACRO:
                            break;

                        case FlightMode.AUTO:
                            /*
                            // 暂时只管起飞。以最大速度上升，直到达到目标高度。
                            if (Altitude < _takeOffTargetAltitude)
                            {
                                Altitude += _scaledMaxMoveInInterval;
                            }
                            else
                            {
                                Mode = FlightMode.LOITER;
                            }
                            */
                            break;

                        case FlightMode.GUIDED:
                            // 王海, 20160317, 指点时也能体感控制若干通道。
                            //考虑不更新这个，好像没必要-xu
                            //UpdateWithChannels();
                            //UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
                            if (FlightControlMode==1)
                                UpdateWithDestination_v2(_targetLat, _targetLng, _targetAlt);
                            else
                                UpdateWithDestination(_targetLat, _targetLng, _targetAlt);
                            break;

                        case FlightMode.STABILIZE:
                        case FlightMode.ALT_HOLD:
                        case FlightMode.LOITER:
                            UpdateWithChannels();
                            break;

                        case FlightMode.RTL:
                            if (Altitude < ReturnToLaunchAltitude && !ReachedDestination(Latitude, Longitude, ReturnToLaunchAltitude))
                            {
                                UpdateWithDestination(Latitude, Longitude, ReturnToLaunchAltitude);
                            }
                            else if (Altitude > ReturnToLaunchAltitude && !ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude))
                            {
                                UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Altitude);
                            }
                            else
                            {
                                UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, ReturnToLaunchAltitude);
                            }
                            break;

                        case FlightMode.CIRCLE:
                            break;

                        case FlightMode.POSITION:
                            break;

                        case FlightMode.LAND:
                            // 王海, 20160317, 降落时也能体感控制若干通道。
                            UpdateWithChannels();
                            // 以最大速度降落，直到高度为 0。
                            if (Altitude > 0)
                            {
                                if (Altitude > _scaledMaxMoveInInterval)
                                {
                                    Altitude -= _scaledMaxMoveInInterval;
                                }
                                else
                                {
                                    Altitude = 0;
                                }
                                Roll = 0;
                                Pitch = 0;
                            }
                            break;

                        case FlightMode.OF_LOITER:
                            break;

                        case FlightMode.TOY:
                            break;
                        //紧急返航
                        case FlightMode.EMERG_RTL:
                            switch(Emergency_Retstatus)
                            {
                                case 0: //等待返航
                                    if ((DateTime.Now- MissionStartTime).TotalSeconds > _rettime)
                                    {
                                        Emergency_Retstatus = 1;
                                        //平飞或降落随机距离
                                        FlyToCoreAsync(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
                                        Mode = FlightMode.EMERG_RTL;
                                    }

                                   
                                    break;
                                case 1: //返航阶段一:等待平飞或降落随机距离并到达,然后飞到起飞点上空
                                        
                                    UpdateWithDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude);
                                    if (ReachedDestination(Emergency_firstloc.Latitude, Emergency_firstloc.Longitude, Emergency_firstloc.Altitude))
                                    {
                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
                                        Mode = FlightMode.EMERG_RTL;
                                        Emergency_Retstatus = 2;
                                    }
                                  
                                    break;
                                case 2: //返航阶段二:等待到达起飞点上空,然后降落

                                    UpdateWithDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
                                    if (ReachedDestination(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude))
                                    {
                                        FlyToCoreAsync(TakeOffPoint.Latitude, TakeOffPoint.Longitude, Emergency_firstloc.Altitude);
                                        //FlyToCoreAsync里面把模式改成了GUIDED，这里改回来
                                        Mode = FlightMode.LAND;
                                        Emergency_Retstatus = 3;
                                    }
                                    break;
                                case 3: //降落
                                    break;
                            }
                            break;

                        default:
                            break;
                    }
                    //UpdateFlightDataIfNeeded();
                }
            }
            else
            {
                // TODO: 王海, 20151228, 模拟空中锁定。
                // 锁定时直接把速度设为 0。
                AirSpeed = 0;
            }

          //  UpdateShowColor();



            _uiSyncContext.Post(() =>
            {
                //位置变化需要在UI刷新
                RaiseLocationChangedIfNeeded();
                //高度变化需要在UI刷新
               // RaiseAltitudeChangedIfNeeded();
                //GPS数据用于显示
              //  RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);
                //不考虑姿态
             //   RaiseAttitudeChanged();
            });
        }

        /// <summary>
        /// 根据各个通道值来更新位置、姿态等状态。
        /// </summary>
        private void UpdateWithChannels()
        {
            var maxMove = Mode == FlightMode.LOITER ? _scaledMaxMoveInInterval : _scaledFastMaxMoveInInterval;
            var ch3Delta = Channel3 > MAX_HOVER_CHANNEL ? Channel3 - MAX_HOVER_CHANNEL : Channel3 < MIN_HOVER_CHANNEL ? Channel3 - MIN_HOVER_CHANNEL : 0;
            if (ch3Delta != 0)
            {
                // 更新相对高度。
                Altitude = Math.Max(0, Altitude + maxMove * ch3Delta / MAX_CHANNEL_DELTA);
            }

            var yawRad = Yaw.DegToRad();
            var metersToLocalLng = GeographyUtils.CalcMetersToLngSpan(Latitude);

            var ch1Delta = Channel1 > MAX_HOVER_CHANNEL ? Channel1 - MAX_HOVER_CHANNEL : Channel1 < MIN_HOVER_CHANNEL ? Channel1 - MIN_HOVER_CHANNEL : 0;
            var xDistance = maxMove * ch1Delta / MAX_CHANNEL_DELTA;
            if (ch1Delta != 0)
            {
                // 更新横滚方向的纬度和经度。
                Latitude -= xDistance * Math.Sin(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
                Longitude += xDistance * Math.Cos(yawRad) * metersToLocalLng;
                // 更新横滚姿态。
                Roll = MAX_TILT_IN_FLIGHT * ch1Delta / MAX_CHANNEL_DELTA;
            }
            else
            {
                Roll = 0;
            }

            var ch2Delta = Channel2 > MAX_HOVER_CHANNEL ? Channel2 - MAX_HOVER_CHANNEL : Channel2 < MIN_HOVER_CHANNEL ? Channel2 - MIN_HOVER_CHANNEL : 0;
            var yDistance = maxMove * ch2Delta / MAX_CHANNEL_DELTA;
            if (ch2Delta != 0)
            {
                // 更新俯仰方向的纬度和经度。
                Latitude -= yDistance * Math.Cos(yawRad) * GeographyUtils.METERS_TO_LAT_SPAN;
                Longitude -= yDistance * Math.Sin(yawRad) * metersToLocalLng;
                // 更新俯仰姿态。
                Pitch = MAX_TILT_IN_FLIGHT * ch2Delta / MAX_CHANNEL_DELTA;
            }
            else
            {
                Pitch = 0;
            }
        }

        /// <summary>
        /// 根据目的地来更新位置、姿态等状态。
        /// </summary>
        private void UpdateWithDestination(ILocation loc)
        {
            UpdateWithDestination(loc.Latitude, loc.Longitude, loc.Altitude);
        }

        /// <summary>
        /// 根据目的地来更新位置、姿态等状态。
        /// </summary>
        private void UpdateWithDestination(double lat, double lng, float alt)
        {
            // 与目标点之间的距离。
            var distance = this.CalcDistance(lat, lng, alt);
            // 距离已经很近，直接移动到目标点。
            if (distance < _scaledMaxMoveInInterval)
            {
                MoveToPointImmediately(lat, lng, alt);
                return;
            }
            // 在空间中的移动距离。
            var move = _scaledMaxMoveInInterval;
            // 竖直方向的移动距离。
            var altDelta = (float)(move * (alt - Altitude) / distance);
            // 更新高度。
            Altitude += altDelta;
            // 目标点相对于当前位置的方向。
            // var direction = this.CalcDirection2D(lat, lng);
            // 水平面上的移动距离。
            var moveInHorizontalPlane = Math.Sqrt(move * move - altDelta * altDelta);
            if (double.IsNaN(moveInHorizontalPlane))
            {
                MoveToPointImmediately(lat, lng, alt);
                return;
            }

            // 更新纬度。
            //Longitude += moveInHorizontalPlane * Math.Sin(direction) * GeographyUtils.CalcMetersToLngSpan(Latitude);
            Longitude += moveInHorizontalPlane * _Lng_delta;
            Latitude += moveInHorizontalPlane * _Lat_delta;
            // 更新经度。
            //Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
        }
        //新版本小航点计算移动位置
        private void UpdateWithDestination_v2(double lat, double lng, float alt)
        {
            //_flytime 总飞行时间 秒
            //_startTicks   开始飞行时间ms
            // _distance_xy  米
            // _distance_z    米
            //当前飞行时间
            if ((lat == Latitude) && (lng == Longitude) && (alt == Altitude)) return;
            currflytime =(float)( DateTime.Now -_startTicks).TotalMilliseconds;//实际飞行时间 ms
            //超时直接移动到目标点
            if (currflytime >= _flytime)
            {
                MoveToPointImmediately(lat, lng, alt);
                return;
            }
            //if (currflytime > 13000)
            //    return;
            // 根据飞行时间计算飞行距离

            float vspeed = 0;

            GC_xy.getspeeddist(_flytype, _flytime / 1000, _distance_xy, currflytime / 1000, acc_xy,maxspeed_xy , out currdis_xy, out vspeed);

            if (alt> Altitude)
                GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_up, out currdis_z, out vspeed);
            else
                GC_z.getspeeddist(_flytype, _flytime / 1000, _distance_z, currflytime / 1000, acc_z,maxspeed_down, out currdis_z, out vspeed);

            Console.WriteLine($"{this.Id}：time:{currflytime / 1000}  to {currdis_z}");

            // 距离已经很近，直接移动到目标点。           
            if ((Math.Abs(_distance_xy-currdis_xy) < 0.1)&& (Math.Abs(_distance_z - currdis_z) < 0.1))
            {
                Console.WriteLine($"{this.Id}  to  tlat:{lat}tlng:{lng}");

                MoveToPointImmediately(lat, lng, alt);
                return;
            }
            // 更新位置
            Altitude = _startAlt+ currdis_z;
            Longitude  = _startLng  + currdis_xy*_Lng_delta;
            Latitude = _startLat + currdis_xy *_Lat_delta;
        }

    }
}