using System;
using static Plane.Protocols.MAVLink;

namespace Plane.Copters
{
    public partial class PLCopter : CopterImplSharedPart
    {
        private bool _fetchingFirmwareVersion;

        private void _internalCopter_ConnectionBroken(object sender, EventArgs e)
        {
            IsConnected = _internalCopter.IsConnected;
        }

        private void _internalCopter_GetLogDataEvent(string log)
        {
            //飞机消息日志，后面需要改为记录方式
            StatusText =Name+":"+log;
        }

        private void _internalCopter_ReceiveDataStreamEvent(object sender, byte StreamType)
        {
            switch (StreamType)
            {
                case MAVLINK_MSG_ID_GPS_RAW_INT:
                    {
                        Latitude = _internalCopter.lat;
                        Longitude = _internalCopter.lng;
                        SatCount = _internalCopter.satcount;
                        GpsFixType = _internalCopter.gpsstatus.ToGpsFixType();
                        GpsHdop = _internalCopter.gpshdop;
                        Elevation = _internalCopter.gpsalt;

                        if (IsGpsAccurate)
                        {
                            UpdateFlightDataIfNeeded();
                            RaiseLocationChangedIfNeeded();
                        }

                        RaiseDataStreamReceived(PDataStreamType.GPS_RAW_INT);

                        break;
                    }
                case MAVLINK_MSG_ID_ATTITUDE:
                    {
                        Roll = _internalCopter.roll;
                        Pitch = _internalCopter.pitch;
                        Yaw = _internalCopter.yaw;
                        TimebootMs = _internalCopter.timebootms;
                        RaiseAttitudeChanged();

                        break;
                    }
                case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
                    {
                        Channel1 = _internalCopter.ch1in;
                        Channel2 = _internalCopter.ch2in;
                        Channel3 = _internalCopter.ch3in;
                        Channel4 = _internalCopter.ch4in;
                        Channel5 = _internalCopter.ch5in;
                        Channel6 = _internalCopter.ch6in;
                        Channel7 = _internalCopter.ch7in;
                        Channel8 = _internalCopter.ch8in;

                        RaiseDataStreamReceived(PDataStreamType.RC_CHANNELS_RAW);

                        break;
                    }
                case MAVLINK_MSG_ID_VFR_HUD:
                    {
                        Heading = _internalCopter.heading;
                        Altitude = _internalCopter.alt;
                        AirSpeed = _internalCopter.airspeed;
                        GroundSpeed = _internalCopter.groundspeed;

                        RaiseAltitudeChangedIfNeeded();

                        break;
                    }
            }
        }

        private async void _internalCopter_ReceiveHeartBearEvent(object sender, uint iError, ulong iCount)
        {
            IsUnlocked = _internalCopter.armed;
            Mode = (FlightMode)_internalCopter.mode;
            // 林俊清, 20160311, 亿航将闲置的 apname 用作已经切过 GPS 模式的标志。
            HasSwitchedToGpsMode = (byte)_internalCopter.apname != 0;
            ++HeartbeatCount;

            IsCheckingConnection = false;
            /*
            if (FirmwareVersion == null && !_fetchingFirmwareVersion)
            {
                try
                {
                    _fetchingFirmwareVersion = true;
                    FirmwareVersion = (int)await GetParamAsync("FIRMWARE_VERSION", 5000);
                }
                catch (TimeoutException)
                {
                    // 吞掉。
                }
                finally
                {
                    _fetchingFirmwareVersion = false;
                }
            }
            */
            RaiseHeartbeatReceived(new HeartbeatReceivedEventArgs(iCount));
        }

        private void _internalCopter_ReceiveSensorEvent(object sender)
        {
            RaiseSensorDataReceived(EventArgs.Empty);
        }

        private void _internalCopter_ReceiveSysStatusEvent(object sender, byte StreamType, bool GPSBadHealth, bool GyroBadHealth, bool AccelBadHealth, bool CompassBadHealth, bool BarometerBadHealth, bool canTakeOff)
        {
            Voltage = _internalCopter.battery_voltage / 1000;

            if (_internalCopter.battery_remaining == 255)   // 剩余电量未知，需要计算。
            {
                CalcBatteryPer();
            }
            else
            {
                BatteryPer = _internalCopter.battery_remaining;
            }

            const int MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD = 12308;
            const int MIN_VERSION_OF_GENERATION2 = 0x3000;
            if (FirmwareVersion != null)
            {
                if (FirmwareVersion.Value >= MIN_VERSION_WITH_CAN_TAKE_OFF_FIELD)
                {
                    // 飞控提供了 canTakeOff。
                    IsGpsAccurate = canTakeOff;
                }
                else if (FirmwareVersion.Value >= MIN_VERSION_OF_GENERATION2)
                {
                    // 2.0 飞行器，飞控未提供 canTakeOff。
                    IsGpsAccurate = SatCount >= 12;
                }
                else
                {
                    // 1.0 飞行器，飞控不提供 canTakeOff。
                    IsGpsAccurate = SatCount >= 6;
                }
            }
            else
                IsGpsAccurate = SatCount >= 8;

            FlightDistance2D = _internalCopter.FlightDistance2D;

            RaiseSysStatusReceived(new SystemStatusReceivedEventArgs(GPSBadHealth, GyroBadHealth, AccelBadHealth, CompassBadHealth, BarometerBadHealth));
        }

        private void _internalCopter_PacketHandled(object sender, PlaneCopter.PacketReceivedEventArgs e)
        {
            switch (e.Packet[5])
            {
                case MAVLINK_MSG_ID_MISSION_COUNT:
                    MissionCount = _internalCopter.WpCount;
                    break;

                default:
                    break;
            }
        }

        private void _internalCopter_UnhandledPacketReceived(object sender, PlaneCopter.PacketReceivedEventArgs e)
        {
            switch (e.Packet[5])
            {
                case MAVLINK_MSG_ID_MISSION_ACK:
                    AnalyzeMissionAckPacket(e.Packet);
                    break;

                case MAVLINK_MSG_ID_MISSION_COUNT:
                    AnalyzeMissionCountPacket(e.Packet);
                    break;

                case MAVLINK_MSG_ID_MISSION_ITEM:
                    AnalyzeMissionItemPacket(e.Packet);
                    break;

                case MAVLINK_MSG_ID_MISSION_REQUEST:
                    AnalyzeMissionRequestPacket(e.Packet);
                    break;

                case MAVLINK_MSG_ID_SET_PAIR:
                    AnalyzeSetPairPacket(e.Packet);
                    break;

                default:
                    break;
            }
        }

#if PRIVATE
        protected virtual void RegisterInternalCopterEventHandlers()
#else

        private void RegisterInternalCopterEventHandlers()
#endif
        {
            _internalCopter.GetLogDataEvent += _internalCopter_GetLogDataEvent;
            _internalCopter.ReceiveDataStreamEvent += _internalCopter_ReceiveDataStreamEvent;
            _internalCopter.ReceiveHeartBearEvent += _internalCopter_ReceiveHeartBearEvent;
            _internalCopter.ReceiveSensorEvent += _internalCopter_ReceiveSensorEvent;
            _internalCopter.ReceiveSysStatusEvent += _internalCopter_ReceiveSysStatusEvent;
            _internalCopter.ConnectionBroken += _internalCopter_ConnectionBroken;
            _internalCopter.PacketHandled += _internalCopter_PacketHandled;
            _internalCopter.UnhandledPacketReceived += _internalCopter_UnhandledPacketReceived;
        }

        #region 计算剩余电量

        private int bPerTimes;
        private int outBatteryPer;
        private int[] tPerTimes = new int[20];
        private int v_num;

        /// <summary>
        /// 计算电量并在条件满足时更新 <see cref="BatteryPer"/> 属性。
        /// </summary>
        private void CalcBatteryPer()
        {
            float volmax = 0f;
            float volmin = 0f;
            if (Voltage > 5 && Voltage < 9)
            {
                volmax = 8.2f;
                volmin = 7f;
            }
            else if (Voltage >= 9
                  && Voltage < 13.6)
            {
                volmax = 11.6f;
                volmin = 10.2f;
            }
            else if (Voltage >= 13.6
                  && Voltage < 17.2)
            {
                volmax = 16.3f;
                volmin = 14.2f;
            }
            else if (Voltage >= 17.2
                  && Voltage < 26.2)
            {
                volmax = 24.8f;
                volmin = 21.2f;
            }

            int batteryPer = (int)Math.Round((Voltage - volmin) * 100 / (volmax - volmin));

            if (batteryPer == -1 || volmax == 0 || volmin == 0)
                return;

            if (bPerTimes < 20)
            {
                tPerTimes[bPerTimes] = batteryPer;
                bPerTimes += 1;
            }
            else
            {
                tPerTimes[v_num] = batteryPer;
                v_num++;
                if (v_num == 20)
                    v_num = 0;
            }
            for (int i = 0; i < bPerTimes; i++)
            {
                outBatteryPer += tPerTimes[i];
            }

            outBatteryPer = outBatteryPer / bPerTimes;
            if (outBatteryPer < BatteryPer && bPerTimes > 18)
            {
                BatteryPer = (byte)outBatteryPer;
            }
        }

        #endregion 计算剩余电量
    }
}
