using Microsoft.FlightSimulator.SimConnect;
using System;
using System.Diagnostics;
using System.Runtime.InteropServices;
using System.Windows;
using System.Windows.Interop;

namespace MD11_Localizer_Capture
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        // User-defined win32 event
        private const int WM_USER_SIMCONNECT = 0x0402;

        private readonly IntPtr Handle;
        private readonly HwndSource HandleSource;

        // Declare a SimConnect object 
        private SimConnect SimConnect = null;

        private readonly Stopwatch stopWatch = new();

        private Struct1? lastFrame = null;

        private bool paused;

        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
        struct Struct1
        {
            public double nav1crs;
            public double nav1cdi;
            public double gspeed;
            public double velWorldX;
            public double velWorldZ;
            public double magVar;
        };

        enum DEFINITIONS
        {
            Struct1,
        }

        enum REQUESTS
        {
            Struct1,
            VOR,
        }

        enum EVENTS
        {
            Pause,
            TglPause,
        }

        enum GROUPS
        {
            Highest = 1,
        }

        public MainWindow()
        {
            InitializeComponent();

            Handle = new WindowInteropHelper(this).EnsureHandle(); // Get handle of main WPF Window
            HandleSource = HwndSource.FromHwnd(Handle); // Get source of handle in order to add event handlers to it
            HandleSource.AddHook(HandleSimConnectEvents);
        }

        ~MainWindow()
        {
            HandleSource?.RemoveHook(HandleSimConnectEvents);
        }

        /// <summary>
        /// Connects / Disconnect from FS
        /// </summary>
        /// <param name="sender">Sender object</param>
        /// <param name="e">Event arguments</param>
        private void ButtonConnection_Click(object sender, RoutedEventArgs e)
        {
            if (SimConnect != null)
            {
                SimConnect.Dispose();
                SimConnect = null;
                lastFrame = null;

                BlueValues.Text = "";

                buttonConnection.Content = "Connect";
                return;
            }

            try
            {
                SimConnect = new SimConnect("MD11 Localizer Capture", Handle, WM_USER_SIMCONNECT, null, 0);

                SimConnect.OnRecvSimobjectData += OnRecvSimobjectData;
                SimConnect.OnRecvEvent += OnRecvEvent;

                SimConnect.SubscribeToSystemEvent(EVENTS.Pause, "PAUSE");

                SimConnect.MapClientEventToSimEvent(EVENTS.TglPause, "KEY_PAUSE_ON ");
                SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.TglPause, false);
                SimConnect.SetNotificationGroupPriority(GROUPS.Highest, SimConnect.SIMCONNECT_GROUP_PRIORITY_HIGHEST);

                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV OBS:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV CDI:1", "Number", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "GROUND VELOCITY", "Feet/Second", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "VELOCITY WORLD X", "Radians", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "VELOCITY WORLD Z", "Radians", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "MAGVAR", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);

                SimConnect.RegisterDataDefineStruct<Struct1>(DEFINITIONS.Struct1);

                SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SIM_FRAME, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
                stopWatch.Start();

                buttonConnection.Content = "Disconnect";
            }
            catch (COMException ex)
            {
                // A connection to the SimConnect server could not be established 
                SimConnect = null;

                MessageBox.Show(ex.ToString());
            }
        }

        /// <summary>
        /// Win32 Message Pump equivalent
        /// </summary>
        /// <param name="hWnd">Window Handel</param>
        /// <param name="message">Message ID</param>
        /// <param name="wParam">Parameters</param>
        /// <param name="lParam">Parameters</param>
        /// <param name="isHandled">WFlag indicating application handeled messag or not</param>
        /// <returns></returns>
        private IntPtr HandleSimConnectEvents(IntPtr hWnd, int message, IntPtr wParam, IntPtr lParam, ref bool isHandled)
        {
            isHandled = false;

            switch (message)
            {
                case WM_USER_SIMCONNECT:
                    {
                        if (SimConnect != null)
                        {
                            SimConnect.ReceiveMessage();
                            isHandled = true;
                        }
                    }
                    break;

                default:
                    break;
            }

            return IntPtr.Zero;
        }

        private void OnRecvSimobjectData(SimConnect sender, SIMCONNECT_RECV_SIMOBJECT_DATA data)
        {
            switch ((REQUESTS)data.dwRequestID)
            {
                case REQUESTS.Struct1:
                {
                    if (paused) return;

                    Struct1 d = (Struct1)data.dwData[0];

                    if (lastFrame.HasValue && lastFrame.Value.nav1cdi != d.nav1cdi)
                    {
                        double deltaT = stopWatch.ElapsedMilliseconds;
                        stopWatch.Restart();

                        double displacement = FCC_GetDisplacement(d, deltaT);
                        double dispRate = FCC_GetDisplacementRate(d, deltaT);

                        if (dispRate != 0.0)
                        {
                            bool blueBox;
                            {
                                double deltaTrack = Math.Abs(d.nav1crs - IRU_GetMagTrack(d));
                                deltaTrack = Math.Max(Math.Min(deltaTrack, 90), -90);
                                deltaTrack = Math.Abs(deltaTrack) * 0.2 / 90.0;
                                double GAIN = 1.0; //tune this?
                                deltaTrack += 0.8 * GAIN;
                                deltaTrack *= dispRate;

                                double scaledDisp = displacement * 0.065;

                                double blue = scaledDisp + deltaTrack;
                                blue *= displacement;

                                blueBox = blue < 0;
                            }

                            bool redBox = Math.Abs(displacement) < 500 && Math.Abs(dispRate) < 25;

                            BlueValues.Text = $"Displacement: {displacement}\n" +
                                              $"Displacement Rate: {dispRate}\n" +
                                              $"BlueBox: {blueBox}\n" +
                                              $"RedBox: {redBox}\n" +
                                              $"Delta T: {deltaT}\n" +
                                              $"Deviation Rate: {FCC_GetDeviationRate(d, deltaT)}";

                            if (redBox || blueBox)
                            {
                                SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.TglPause, 1, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
                            }
                        }

                    }
                    lastFrame = d;

                    break;
                }
                default: break;
            }
        }

        private void OnRecvEvent(SimConnect sender, SIMCONNECT_RECV_EVENT data)
        {
            switch ((EVENTS)data.uEventID)
            {
                case EVENTS.Pause:
                {
                    paused = data.dwData == 1;
                    if (data.dwData == 0) stopWatch.Start();
                    else stopWatch.Stop();
                    break;
                }
                default: break;
            }
        }

        #region Collin Funcs

        // This just returns *magentic* track
        double IRU_GetMagTrack(Struct1 data) {
            double trk = ToDegrees(Math.Atan2(data.velWorldX, data.velWorldZ));
            return (trk < 0 ? trk + 360 : trk) - data.magVar;
        }

        double FCC_GetDisplacement(Struct1 data, double lastCdiTime)
        {
            double rate = FCC_GetDeviationRate (data, lastCdiTime);
            if (rate != 0)
            {
                // Move division into Abs
                double distToIntercept = Math.Abs(data.nav1cdi / rate);
                // My GSpeed is in ft/s, dunno if that migh cause issues (shouldn't but eh)
                distToIntercept *= data.gspeed;

                double deltaTrack = Math.Abs(data.nav1crs - IRU_GetMagTrack(data));
                double displacement = distToIntercept * Math.Sin(ToRadians(deltaTrack));

                // Return Absolute value
                return Math.Abs(displacement);
            }
            return 0.0;
        }

        double FCC_GetDisplacementRate(Struct1 data, double lastCdiTime)
        {
            if (lastFrame.HasValue && lastFrame.Value.nav1cdi != data.nav1cdi && lastCdiTime != 0)
            {
                // Switch subtraction
                // Otherwise, at least for me, the sign of the output was inverted (- for drifitng away instead of +)
                double deltaDev =  lastFrame.Value.nav1cdi - data.nav1cdi;
                double deltaTrack = Math.Abs(data.nav1crs - IRU_GetMagTrack(data));
                double dispRate = (deltaDev < 0 ? -1 : 1) * data.gspeed * Math.Abs(Math.Sin(ToRadians(deltaTrack)));

                return dispRate;
            }
            return 0.0;
        }

        double FCC_GetDeviationRate(Struct1 data, double lastCdiTime)
        {
            if (lastFrame.HasValue && lastFrame.Value.nav1cdi != data.nav1cdi && lastCdiTime != 0)
            {
                // Switch subtraction
                // Adjust for ms to s by multiplying by th time step expressed as seconds
                double devRate = (lastFrame.Value.nav1cdi - data.nav1cdi) * (1000 / lastCdiTime);
                return Math.Abs(devRate);
            }
            return 0.0;
        }

        #endregion

        #region Helpers

        private double ToRadians(double degrees) => (Math.PI / 180) * degrees;

        private double ToDegrees(double radians) => (180/ Math.PI) * radians;

        #endregion
    }
}