From e271e49374be42c6e8b2cbd02e99606996c52017 Mon Sep 17 00:00:00 2001
From: Luke Else <mail@luke-else.co.uk>
Date: Fri, 18 Feb 2022 21:13:21 +0000
Subject: [PATCH] Added closest waypoint function

---
 Controllers/FlightsimController.cs | 41 +++++++++++++++++++++++++++---
 1 file changed, 38 insertions(+), 3 deletions(-)

diff --git a/Controllers/FlightsimController.cs b/Controllers/FlightsimController.cs
index 90ee24f..f4468e3 100644
--- a/Controllers/FlightsimController.cs
+++ b/Controllers/FlightsimController.cs
@@ -8,6 +8,7 @@ using EFB.Sessions;
 using Microsoft.Extensions.Logging;
 using EFB.Models;
 using EFB.MongoData;
+using EFB.Models.Route;
 
 namespace EFB.Controllers
 {
@@ -30,13 +31,47 @@ namespace EFB.Controllers
             //Retrieve the user's latest sim position and construct it into FlightsimModel
             if (user.Route == null) return RedirectToAction("Index", "Route");
 
-            SimPositionModel latestPosition = await Mongo.GetLatestData(user.EMail);
-            RouteModel route = await RouteModel.StringToRoute(user.Departure, user.Arrival, user.Cruise, user.Route);
+            SimPositionModel latestPositionModel = await Mongo.GetLatestData(user.EMail);
 
-            return View(new FlightsimModel(latestPosition, null));
+            RouteModel route = await RouteModel.StringToRoute(user.Departure, user.Arrival, user.Cruise, user.Route);
+            IWaypoint closest = DetermineClosest(route, latestPositionModel.LatestPosition);
+
+            return View(new FlightsimModel(latestPositionModel, closest));
 
         }
 
+        private IWaypoint DetermineClosest(RouteModel route, SimPosition currentPosition){
+            IWaypoint closest = null;
+            float closestDistance = float.MaxValue;
+            //Assuming that we are on the earth for simplicity
+            IWaypoint waypoint = route.Departure;
+            while (waypoint.Next != null)
+            {
+                int earthRadius = 6371;
+                float distanceLat = DegreesToRadians(waypoint.Latitude - currentPosition.Latitude);
+                float distanceLon = DegreesToRadians(waypoint.Longitude - currentPosition.Longitude);
+
+                float latitude1 = DegreesToRadians(currentPosition.Latitude);
+                float latitude2 = DegreesToRadians(waypoint.Latitude);
+
+                var a = Math.Sin(distanceLat/2) * Math.Sin(distanceLat/2) + Math.Sin(distanceLon/2) * Math.Sin(distanceLon/2) * Math.Cos(latitude1) * Math.Cos(latitude2);
+                var c = 2 * Math.Atan2(Math.Sqrt(a), Math.Sqrt(1-a));
+
+                float distance = (float)(earthRadius * c);
+
+                if (distance < closestDistance)
+                {
+                    closest = waypoint;
+                    closestDistance = distance;
+                }
+                waypoint = waypoint.Next;
+            }
+            return closest;
+        }
+        private float DegreesToRadians(float degrees){
+            return (float)(degrees * Math.PI / 180);
+        } 
+
         [ResponseCache(Duration = 0, Location = ResponseCacheLocation.None, NoStore = true)]
         public IActionResult Error()
         {