using Plane.Copters;
using Microsoft.Practices.ServiceLocation;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Plane.Geography;

namespace Plane.FormationCreator.Formation
{
    public partial class FlightTask
    {


        //起飞到第一个航点高度的飞行时间
        private byte _TakeOffTime = 10;
        public byte TakeOffTime
        {
            get { return _TakeOffTime; }
            set { Set(nameof(TakeOffTime), ref _TakeOffTime, value); }
        }

        //一架架起飞--未使用
        public async Task RunTakeOffTaskAsync()
        {
            // float takeOffAlt = 15;
            int TaskCount = _flightTaskManager.Tasks.Count();
            if (TaskCount > 1)
            {
                var infos = SingleCopterInfos;
                // var tasks = new Task[infos.Count];
                var tasksTmp = new Task[infos.Count];
                for (int i = 0; i < infos.Count; i++)
                {
                    var info = infos[i];
                    
                    if (info.takeOffStage == 0)   // 第一阶段：起飞到10m
                    {
                        await TakeOffTaskFlySingleCopterAsync(info);
                        //if (_flightTaskManager.IsPaused == false)
                        //{
                        //    info.takeOffStage++;
                        //}
                    }
                    
                    tasksTmp[i] = TakeOffSecondTaskAsync(info);   // 第二和第三阶段
                }
                await Task.WhenAll(tasksTmp).ConfigureAwait(false);
                if (_flightTaskManager.IsPaused == false)
                {
                    for (int i = 0; i < infos.Count; i++)
                    {
                        var info = infos[i];
                        info.takeOffStage = 0;
                    }
                }
                
            }
        }

        //新版的起飞方案
        public async Task NewMutilRunTakeOffTaskAsync()
        {
            var infos = SingleCopterInfos;
            var tasksTakeOff = new Task[infos.Count];
            for (int i = 0; i < infos.Count; i++)
            {
                //tasksTakeOff[i] = NewSingleRunTaskOffTaskAsunc(infos[i]);


                tasksTakeOff[i] = await Task.Factory.StartNew(async () =>
                {
                    var internalInfo = infos[i];

                    await NewSingleRunTaskOffTaskAsunc(internalInfo);
                });
            }
            await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
            //await Task.Delay(100);
        }

        private async Task NewSingleRunTaskOffTaskAsunc(FlightTaskSingleCopterInfo info)
        {
            DateTime dtNow = DateTime.Now;
            DateTime dtLastTime = DateTime.Now;
            TimeSpan ts = dtNow - dtLastTime;

            int copterIndex = SingleCopterInfos.IndexOf(info);
            var copter = info.Copter;
            await copter.UnlockAsync();
            //等待起飞时间
            while ((int)ts.TotalMilliseconds < (int)info.TakeOffWaitTime * 1000)
            {
                if (_flightTaskManager.IsPaused == true)
                {
                    await copter.UnlockAsync();
                    await info.Copter.HoverAsync();
                    return;
                }
                await Task.Delay(100);
                dtNow = DateTime.Now;
                ts = dtNow - dtLastTime;   
            }

            //虚拟飞机5秒后不起飞会自动上锁
            await copter.UnlockAsync();

            for (int i = 0; i < 5; i++)   // added by ZJF
            {
                await copter.TakeOffAsync();
                await Task.Delay(50).ConfigureAwait(false);
            }

            var copterNextTask = _flightTaskManager.Tasks[TaskIndex + 1].SingleCopterInfos[copterIndex];
            float takeOffAlt = copterNextTask.TargetAlt;

            info.TargetLat = info.Copter.Latitude;
            info.TargetLng = info.Copter.Longitude;

          //  for (int j = 0; j < 3; j++)
          //  {
                await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt);
             //   await Task.Delay(10).ConfigureAwait(false);
           // }
            //解锁起飞用暗紫色
            info.Copter.LEDColor = "FF00FF";

            dtNow = DateTime.Now;
            ts = dtNow - dtLastTime;
            FlightTask task = _flightTaskManager.CurrentRunningTask;

            while (ts.TotalMilliseconds < task.TakeOffTime * 1000)
            {
                if (_flightTaskManager.IsPaused == true)
                {
                    await info.Copter.HoverAsync();
                    return;
                }
                await Task.Delay(100).ConfigureAwait(false);
                dtNow = DateTime.Now;
                ts = dtNow - dtLastTime;
            }
            //起飞完成用暗蓝色
            info.Copter.LEDColor = "0000FF";

        }


        //老方案 ----- 按起飞数量起飞
        // 几架飞机同时起飞，参数为takeOffCount-----------------使用中
        //起飞分三个阶段：
        //1阶段：分批起飞到15米(目前15米高度是飞控起飞航点决定)，上一批起飞超过5米下一批开始起飞，
        //       等待全部起飞完成后执行第二阶段，
        //2阶段：等待高度超过9米(可能已到达15米)然后平飞到第一个航点位置,高度为15米的位置，
        //3阶段：垂直上升到第一个航点指定高度
        //
        //修改方案：针对高度
        //1.上升到起飞高度(目前没有地面站设置高度，高度在飞控中)
        //2.直接飞往第一个航点的高度
        public async Task MutilRunTakeOffTaskAsync()
        {
            
            int TaskCount = _flightTaskManager.Tasks.Count();
            if (TaskCount > 1)
            {
                var infos = SingleCopterInfos;

                //不再使用起飞数量  强制设置起飞总数等于所有飞机 
                int takeOffCount = _copterManager.Copters.Count();
                int copterCount = infos.Count;
                int integerPart = copterCount / takeOffCount;
                int residualPart = copterCount % takeOffCount;

                // var tasks = new Task[infos.Count];
                var tasksTmp = new Task[infos.Count];
                for (int i = 0; i < integerPart; i++)
                {
                    var tasksTakeOff = new Task[takeOffCount];
                    //执行n架同时起飞
                    for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
                    {
                        var info = infos[j];

                        int indexTmp = j - takeOffCount * i;
                        if (info.takeOffStage == 0)   // 第一阶段：起飞到15m
                        {
                            tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
                        }
                        else
                        {
                            tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
                        }
                    }
                    //等待多架起飞任务-起飞高度超过5米完成
                    await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
                    //加入已起飞飞机的第二第三阶段起飞任务--并不执行
                    for (int j = takeOffCount * i; j < takeOffCount * (i + 1); j++)
                    {
                        var info = infos[j];
                        tasksTmp[j] = TakeOffSecondTaskAsync(info);   // 第二和第三阶段
                    }
                }

                // 余数架飞机同时起飞
                if (residualPart > 0)
                {
                    var tasksTakeOff = new Task[residualPart];
                    for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++)
                    {
                        var info = infos[j];

                        int indexTmp = j - takeOffCount * integerPart;
                        if (info.takeOffStage == 0)   // 第一阶段：起飞到15m
                        {
                            tasksTakeOff[indexTmp] = TakeOffTaskFlySingleCopterAsync(info);
                        }
                        else
                        {
                            tasksTakeOff[indexTmp] = Task.Run(async () => { await Task.Delay(1).ConfigureAwait(false); });
                        }
                    }
                    //等待起飞超过5米
                    await Task.WhenAll(tasksTakeOff).ConfigureAwait(false);
                    
                    for (int j = integerPart * takeOffCount; j < takeOffCount * integerPart + residualPart; j++)
                    {
                        var info = infos[j];
                        tasksTmp[j] = TakeOffSecondTaskAsync(info);   // 加入第二和第三阶段
                    }
                }
                //执行并等待所有的第二第三阶段起飞任务完成
                await Task.WhenAll(tasksTmp).ConfigureAwait(false);
                if (_flightTaskManager.IsPaused == false)
                {
                    for (int i = 0; i < infos.Count; i++)
                    {
                        var info = infos[i];
                        info.takeOffStage = 0;
                    }
                }

            }
        }
        
        private CopterManager _copterManager = ServiceLocator.Current.GetInstance<CopterManager>();

        //执行第一阶段解锁起飞任务---使用中
        private async Task TakeOffTaskFlySingleCopterAsync(FlightTaskSingleCopterInfo info)
        {
            DateTime dtNow = DateTime.Now;
            DateTime dtLastTime = DateTime.Now;
            TimeSpan ts = dtNow - dtLastTime;

            int copterIndex = SingleCopterInfos.IndexOf(info);
            var copter = info.Copter;
            var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];

            if ((bool)_copterManager.CopterStatus[copterIndex])
                return;

            if (_flightTaskManager.IsPaused == true)
            {
                return;
            }

            //设置灯光
            await copter.SetShowLEDAsync(true);
            
            //开始解锁
            await copter.UnlockAsync();
            for (int i = 0; !copter.IsUnlocked; i++)
            {
                //if (_flightTaskManager.IsPaused == true)
                //{
                //    return;
                //}
                //8秒内每1000毫秒尝试解锁一次
                //解锁间隔一定要超过1s否则导致飞控以后无法解锁

                if (i > 320)
                    return;  //无法解锁后面不用执行了
                if (i % (1000 / 25) == 1000 / 25 - 1)
                {
                    await copter.UnlockAsync(); // 每 1000 毫秒重试一次。
                }

                await Task.Delay(25).ConfigureAwait(false);
            }
            //////解锁完成

            // 为了返航，记录家的位置, 应该放在起飞命令
            info.TargetLat = info.Copter.Latitude;
            info.TargetLng = info.Copter.Longitude;

            //等待起飞时间
            while (ts.TotalMilliseconds < info.TakeOffWaitTime * 1000 )
            {
                await Task.Delay(100);
                dtNow = DateTime.Now;
                ts = dtNow - dtLastTime;
            }

            //开始起飞
            for (int i = 0; i < 5; i++)   // added by ZJF
            {
                await copter.TakeOffAsync();
                await Task.Delay(50).ConfigureAwait(false);
            }

            // while (copter.Altitude < 4 || copter.State == Copters.CopterState.TakingOff)
            //低于5米任务一直等待
            while (copter.Altitude < 5)                 // 修改起飞逻辑，当高度达到5米时，下一架开始起飞
            {
                if (_flightTaskManager.IsPaused == true)
                {
                    await info.Copter.HoverAsync();
                    return;
                }
                await Task.Delay(100).ConfigureAwait(false);
            }

/*         //先不要这个控制看能否正常工作  
           if (copter.Altitude > 8)
            {
                await info.Copter.GuidAsync();
                return;
            }
*/
        }

        //执行第二第三阶段起飞任务-------------使用中
        private async Task TakeOffSecondTaskAsync(FlightTaskSingleCopterInfo info)
        {
            
            int copterIndex = SingleCopterInfos.IndexOf(info);
            var copterNextTask = _flightTaskManager.Tasks[1].SingleCopterInfos[copterIndex];
            float takeOffAlt = copterNextTask.TargetAlt;// 15; 起飞高度到下个任务的高度
            // 当该飞机被标记时，悬停并跳过飞行任务
            if ((bool)_copterManager.CopterStatus[copterIndex])
            {
                await info.Copter.HoverAsync();
                return;
            }            // await Task.Run(async () =>
            // {
            // 小于9米时等待----按起飞高度算此时可能已到达15米起飞高度
            if (info.takeOffStage == 0)
            {
                while (info.Copter.Altitude < 9)
                {
                    if (_flightTaskManager.IsPaused == true)
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                    await Task.Delay(100).ConfigureAwait(false);
                    // 当该飞机被标记时，悬停并跳过飞行任务
                    if ((bool)_copterManager.CopterStatus[copterIndex])
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                }
                if (_flightTaskManager.IsPaused == false)
                {
                    info.takeOffStage++;
                }
            }
            
            DateTime dtNow = DateTime.Now;
            DateTime dtLastTime = DateTime.Now;
            TimeSpan ts = dtNow - dtLastTime;

            #region 屏蔽掉第二个阶段直接进入第三个阶 到达起飞高度直接飞往第一个航点
           
            // 第二阶段：超过9米开始水平飞行
            if (info.takeOffStage == 1)
            {
                //切换到guided模式
                await info.Copter.GuidAsync();
                //设置灯光为航点要求的灯光
                await info.Copter.SetShowLEDAsync(copterNextTask.FlytoShowLED );
                //异步执行飞到第一个航点高度固定为15米的任务
                for (int j = 0; j < 3; j++)
                {
                    await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt);
                    await Task.Delay(10).ConfigureAwait(false);
                }
                //直到到达第一个航点并高度15米
                while (!info.Copter.ArrivedTarget(info.TargetLat, info.TargetLng, takeOffAlt))
                {
                    if (_flightTaskManager.IsPaused == true)
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                    await Task.Delay(25).ConfigureAwait(false);

                    dtNow = DateTime.Now;
                    ts = dtNow - dtLastTime;
                    //每2秒再异步执行一次到航点高度15米的任务
                    if (ts.TotalMilliseconds > 2000)
                    {
                        for (int j = 0; j < 2; j++)
                        {
                            await info.Copter.FlyToAsync(info.TargetLat, info.TargetLng, takeOffAlt);
                            await Task.Delay(10).ConfigureAwait(false);
                        }
                        dtLastTime = dtNow;
                    }
                    // 当该飞机被标记时，悬停并跳过飞行任务
                    if ((bool)_copterManager.CopterStatus[copterIndex])
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                }
                if (_flightTaskManager.IsPaused == false)
                {
                    info.takeOffStage++;   //当前变为2
                }
            }
            
            dtNow = DateTime.Now;
            dtLastTime = DateTime.Now;
            ts = dtNow - dtLastTime;
            
            #endregion

            info.takeOffStage++;

            // 第三阶段：从第一个航点位置15米垂直飞行
            if (info.takeOffStage == 2)
            {
                //执行飞到第一个航点高度为设定目标高度
                for (int j = 0; j < 3; j++)
                {
                    await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
                    await Task.Delay(10).ConfigureAwait(false);
                }
                //直到到达第一个航点
                while (!info.Copter.ArrivedTarget(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt))
                {
                    if (_flightTaskManager.IsPaused == true)
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                    await Task.Delay(25).ConfigureAwait(false);

                    dtNow = DateTime.Now;
                    ts = dtNow - dtLastTime;
                    //每2秒发一次第一个航点飞行任务
                    if (ts.TotalMilliseconds > 2000)
                    {
                        for (int j = 0; j < 2; j++)
                        {
                            await info.Copter.FlyToAsync(copterNextTask.TargetLat, copterNextTask.TargetLng, copterNextTask.TargetAlt);
                            await Task.Delay(10).ConfigureAwait(false);
                        }
                        dtLastTime = dtNow;
                    }
                    // 当该飞机被标记时，悬停并跳过飞行任务
                    if ((bool)_copterManager.CopterStatus[copterIndex])
                    {
                        await info.Copter.HoverAsync();
                        return;
                    }
                }
            }
                
            // });
        }

    }
}