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 const double EarthRadiusInFeet = 20902230;

        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 double lastCalcDispTrue = 0;

        private bool paused;

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

            public SIMCONNECT_DATA_LATLONALT navPos;
            public SIMCONNECT_DATA_LATLONALT pos;
            public int code;
            public double navMagVar;
            public double locCrs;
        };

        enum DEFINITIONS
        {
            Struct1,
        }

        enum REQUESTS
        {
            Struct1,
            VOR,
        }

        enum EVENTS
        {
            Pause,
            AP_NAV1_HOLD_ON,
            AP_NAV_SELECT_SET,
        }

        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;

                NAV1Freq.Content = "";
                NAV1Crs.Content = "";
                NAV1Dev.Content = "";
                GSPD.Content = "";
                TRK.Content = "";
                DeltaT.Content = "";
                BlueValues.Text = "";
                DispRate.Content = "";
                Disp.Content = "";
                Capture.Content = "";
                DeltaDev.Content = "";
                TimeToCenter.Content = "";
                BlueValues.Text = "";
                BlueValuesFake.Text = "";
                ExpValues.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.OnRecvVorList += OnRecvVorList;

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

                SimConnect.MapClientEventToSimEvent(EVENTS.AP_NAV1_HOLD_ON, "AP_NAV1_HOLD_ON");
                SimConnect.MapClientEventToSimEvent(EVENTS.AP_NAV_SELECT_SET, "AP_NAV_SELECT_SET");
                SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.AP_NAV1_HOLD_ON, false);
                SimConnect.AddClientEventToNotificationGroup(GROUPS.Highest, EVENTS.AP_NAV_SELECT_SET, false);
                SimConnect.SetNotificationGroupPriority(GROUPS.Highest, SimConnect.SIMCONNECT_GROUP_PRIORITY_HIGHEST);

                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV ACTIVE FREQUENCY:1", "MHz", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                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);

                // for just sim based data
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV VOR LATLONALT:1", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "STRUCT LATLONALT", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV CODES:1", null, SIMCONNECT_DATATYPE.INT32, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV MAGVAR:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "NAV LOCALIZER:1", "Degrees", SIMCONNECT_DATATYPE.FLOAT64, 0, SimConnect.SIMCONNECT_UNUSED);
                // for navigraph
                // SimConnect.AddToDataDefinition(DEFINITIONS.Struct1, "STRUCT LATLONALT", null, SIMCONNECT_DATATYPE.LATLONALT, 0, SimConnect.SIMCONNECT_UNUSED);

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

                // Calculation using actual sim deviation only works if set to once every second
                SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SIM_FRAME, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
                //SimConnect.RequestDataOnSimObject(REQUESTS.Struct1, DEFINITIONS.Struct1, SimConnect.SIMCONNECT_OBJECT_ID_USER, SIMCONNECT_PERIOD.SECOND, SIMCONNECT_DATA_REQUEST_FLAG.DEFAULT, 0, 0, 0);
                stopWatch.Start();

                SimConnect.SubscribeToFacilities(SIMCONNECT_FACILITY_LIST_TYPE.VOR, REQUESTS.VOR);

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

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

        private void OnRecvVorList(SimConnect sender, SIMCONNECT_RECV_VOR_LIST data)
        {
            switch ((REQUESTS)data.dwRequestID)
            {
                case REQUESTS.VOR:
                {
                    foreach (SIMCONNECT_DATA_FACILITY_VOR vor in data.rgData)
                    {
                        var v = vor;
                    }
                    break;
                }
                default: break;
            }
        }

        /// <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:
                {
                    double deltaT = stopWatch.ElapsedMilliseconds;
                    stopWatch.Restart();

                    if (paused) return;

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

                    // Actual:
                    double dev = Math.Abs(d.nav1cdi) * (155.0 / 127.0);
                    double _trk = ToDegrees(Math.Atan2(d.velWorldX, d.velWorldZ));
                    double trk = (_trk < 0 ? _trk + 360 : _trk) - d.magVar;
                    double deltaTrk = d.nav1crs - trk;

                    //Faked:
                    double distToLoc = CalculateDistance(d.pos.Latitude, d.pos.Longitude, d.navPos.Latitude, d.navPos.Longitude);
                    double magVar = d.navMagVar > 180 ? d.navMagVar - 360 : d.navMagVar;
                    double brgToLoc = CalculateBearing(d.pos.Latitude, d.pos.Longitude, d.navPos.Latitude, d.navPos.Longitude);
                    double calcDisp = distToLoc * Math.Sin(ToRadians(Math.Abs(d.nav1crs - (brgToLoc + magVar))));
                    double calcDispTrue = distToLoc * Math.Sin(ToRadians(Math.Abs(d.locCrs - brgToLoc)));

                    if (lastFrame.HasValue)
                    {
                        // Faked cont.
                        double calcDispRate = ((calcDispTrue - lastCalcDispTrue) < 0 ? -1 : 1) * d.gspeed * Math.Sin(ToRadians(Math.Abs(d.locCrs - trk)));

                        ExpValues.Text = $"Dist to LOC: {distToLoc} ft\n" +
                                         $"True BRG to LOC: {brgToLoc}°\n" +
                                         $"Calced Disp Rate: {calcDispRate} ft/s\n" +
                                         $"TRU LOC CRS: {d.locCrs}°\n" +
                                         $"Calced Disp User CRS: {calcDisp} ft\n" +
                                         $"Calced Disp True CRS: {calcDispTrue} ft\n";

                        // Actual cont.
                        double lastDev = Math.Abs(lastFrame.Value.nav1cdi) * (155.0 / 127.0);
                        double deltaDev = (dev - lastDev) * (1000 / deltaT);
                        // Requires a deviation rate != 0 to calculate properly
                        double timeToCenter = Math.Abs(dev / deltaDev);
                        double dispRate = (deltaDev < 0 ? -1 : 1) * d.gspeed * Math.Sin(ToRadians(Math.Abs(deltaTrk)));
                        // Requires dispRate > 0 to calculate properlys
                        double disp = timeToCenter * Math.Abs(dispRate);

                        bool blueCaptureOG = BlueCheck(deltaTrk, dispRate, disp);
                        bool redCaptureOG = RedCheck(dispRate, disp);

                        bool blueCaptureFK = BlueCheck(deltaTrk, calcDispRate, calcDispTrue, true);
                        bool redCaptureFK = RedCheck(calcDispRate, calcDispTrue);

                        // Check for Full course, Half course and Quarter course nominal widths at ILS reference point
                        bool dispValid = !(double.IsInfinity(disp) ||
                                           double.IsNaN(disp) || 
                                           (dev >= 155 && disp < 350) ||
                                           (dev >= 77.5 && disp < 175) ||
                                           (dev >= 38.75 && disp < 87.5));

                        if (deltaDev != 0)
                        {
                            DispRate.Content = $"Disp. rate: {calcDispRate} ft/s";
                            Disp.Content = $"Disp.: {calcDispTrue} ft, VALID OG: {dispValid}";
                            Capture.Content = $"Capture criteria: BLUE OG: {blueCaptureOG}, BLUE FK: {blueCaptureFK}, RED OG: {redCaptureOG}, RED FK: {redCaptureFK}";
                            DeltaDev.Content = $"Deltas: Dev: {deltaDev} DDM/s, TRK: {deltaTrk}";
                            TimeToCenter.Content = $"Time to Center: {timeToCenter} s";
                        }

                        // Use faked values cause they are more accurate
                        if (blueCaptureFK || redCaptureFK)
                        {
                            SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.AP_NAV_SELECT_SET, 1, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
                            SimConnect.TransmitClientEvent(SimConnect.SIMCONNECT_OBJECT_ID_USER, EVENTS.AP_NAV1_HOLD_ON, 0, GROUPS.Highest, SIMCONNECT_EVENT_FLAG.DEFAULT);
                        }
                        
                    }
                    lastFrame = d;
                    lastCalcDispTrue = calcDispTrue;

                    NAV1Freq.Content = $"NAV1 Freq: {d.nav1freq} MHz";
                    NAV1Crs.Content = $"NAV1 Crs: {d.nav1crs}°";
                    NAV1Dev.Content = $"NAV1 DDM: {dev} DDM";
                    GSPD.Content = $"GRND SPD: {d.gspeed} ft/s";
                    TRK.Content = $"TRK MAG: {trk}°";
                    DeltaT.Content = $"Delta t: {deltaT} ms";

                    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 Capture Criteria

        private bool BlueCheck(double deltaTrk, double dispRate, double disp, bool fake = false)
        {
            double normDeltaTrack = (Math.Abs(Math.Max(Math.Min(deltaTrk, 90), -90)) * 0.2) / 90;
            double gainedNormDeltaTrack = normDeltaTrack + (Gain.Value * 0.8);
            double dispRateMulGainedNormDeltaTrack = gainedNormDeltaTrack * dispRate;
            double rescaledDisp = disp * 0.065;
            double preValue = rescaledDisp + dispRateMulGainedNormDeltaTrack;
            double value = preValue * disp;

            if (fake)
            {
                BlueValuesFake.Text = $"Norm Delta TRK: {normDeltaTrack}\n" +
                                      $"Gained Norm Delta TRK: {gainedNormDeltaTrack}\n" +
                                      $"Disp rate Mul Gained Norm Delta TRK: {dispRateMulGainedNormDeltaTrack}\n" +
                                      $"Rescaled Displacement: {rescaledDisp}\n" +
                                      $"Pre value: {preValue}\n" +
                                      $"Value: {value}";
            }
            else
            {
                BlueValues.Text = $"Norm Delta TRK: {normDeltaTrack}\n" +
                                  $"Gained Norm Delta TRK: {gainedNormDeltaTrack}\n" +
                                  $"Disp rate Mul Gained Norm Delta TRK: {dispRateMulGainedNormDeltaTrack}\n" +
                                  $"Rescaled Displacement: {rescaledDisp}\n" +
                                  $"Pre value: {preValue}\n" +
                                  $"Value: {value}";
            }
            return value < 0;
        }

        private bool RedCheck(double dispRate, double disp)
        {
            return Math.Abs(dispRate) < 25 && disp < 500;
        }

        #endregion

        #region Helpers

        private double CalculateDistance(double lat1, double lon1, double lat2, double lon2)
        {
            double dLat = ToRadians(lat2 - lat1);
            double dLon = ToRadians(lon2 - lon1);
            double lat1Rad = ToRadians(lat1);
            double lat2Rad = ToRadians(lat2);
            double sinDLat = Math.Sin(dLat / 2);
            double sinDLon = Math.Sin(dLon / 2);

            double a = sinDLat * sinDLat + sinDLon * sinDLon * Math.Cos(lat1Rad) * Math.Cos(lat2Rad);
            double c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1 - a));

            return EarthRadiusInFeet * c;
        }

        private double CalculateBearing(double lat1, double lon1, double lat2, double lon2)
        {
            double dLon = ToRadians(lon2 - lon1);
            double lat1Rad = ToRadians(lat1);
            double lat2Rad = ToRadians(lat2);

            double y = Math.Sin(dLon) * Math.Cos(lat2Rad);
            double x = Math.Cos(lat1Rad) * Math.Sin(lat2Rad) - Math.Sin(lat1Rad) * Math.Cos(lat2Rad) * Math.Cos(dLon);
            double bearing = Math.Atan2(y, x);

            // Convert bearing from radians to degrees
            bearing *= (180 / Math.PI);

            // Normalize bearing to range from 0 to 360 degrees
            return (bearing + 360) % 360;
        }

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

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

        #endregion
    }
}