using Plane.Communication;
using Plane.Geography;
using System;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;
using static Plane.Copters.Constants;

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 =50;  //默认100， i7电脑 150可以跑1000架 100可以跑500架 50可以200-300架 

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

        /// <summary>
        /// 高速模式下，在一个更新间隔中的最大移动距离。
        /// </summary>
        private  float MAX_MOVE_IN_INTERVAL_FAST = MAX_VEL * UPDATE_INTERVAL / 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 / 1000 * 4;  //MAX_MOVE_IN_INTERVAL_FAST

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

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

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

        /// <summary>
        /// FlyTo 的目标高度。
        /// </summary>
        private float _targetAlt;

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

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


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




        /// <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;

            /*
            // 持续假装收到飞行器发来的心跳。
            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; } }

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

        public Task DisconnectAsync()
        {
            IsConnected = false;
            _isRunning = false;
            IsCheckingConnection = false;
            return TaskUtils.CompletedTask;
        }

        protected override Task FlyToCoreAsync(double lat, double lng, float alt)
        {
            if (!IsEmergencyHoverActive)
            {
                _targetLat = lat;
                _targetLng = lng;
                _targetAlt = alt;
                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);
        }
        public Task MissionStartAsync(int hour_utc, int minute_utc, int second_utc, double Missionlng, double Missionlat)
        {
            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;

        }


        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);
                            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;

                        default:
                            break;
                    }
                    UpdateFlightDataIfNeeded();
                }
            }
            else
            {
                // TODO: 王海, 20151228, 模拟空中锁定。
                // 锁定时直接把速度设为 0。
                AirSpeed = 0;
            }
            _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);
            // 更新经度。
            Latitude += moveInHorizontalPlane * Math.Cos(direction) * GeographyUtils.METERS_TO_LAT_SPAN;
        }
    }
}