HexapiBackground.Navigation.Navigator.NavigateToWaypoint C# (CSharp) 메소드

        internal async Task<bool> NavigateToWaypoint(LatLon currentWaypoint, CancellationToken cancelationToken)
        {
            var distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon);
            var distanceToWaypoint = distanceHeading[0];
            var headingToWaypoint = distanceHeading[1];

            var travelLengthX = 0D;
            var travelLengthZ = 0D;
            var travelRotationY = 0D;
            var nomGaitSpeed = 50D;
            
            travelLengthZ = -50;

            var turnDirection = "None";

            while (distanceToWaypoint > 10) //Inches
            {
                if (cancelationToken.IsCancellationRequested)
                    return false;

                await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1);
                await _display.WriteAsync($"{turnDirection} {Gps.Gps.CurrentLatLon.Heading}", 2);

                if (headingToWaypoint + 5 > 359 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1)
                {
                    var tempHeading = (headingToWaypoint + 5) - 359;

                    if (Gps.Gps.CurrentLatLon.Heading > tempHeading)
                    {
                        turnDirection = "Right";
                        travelRotationY = -1;
                    }
                    else
                    {
                        turnDirection = "Left";
                        travelRotationY = 1;
                    }
                }
                else if (headingToWaypoint - 5 < 1 && Math.Abs(headingToWaypoint - Gps.Gps.CurrentLatLon.Heading) > 1)
                {
                    var tempHeading = (headingToWaypoint + 359) - 5;

                    

                    if (Gps.Gps.CurrentLatLon.Heading < tempHeading)
                    {
                        turnDirection = "Right";
                        travelRotationY = 1;
                    }
                    else
                    {
                        turnDirection = "Left";
                        travelRotationY = -1;
                    }
                }
                else if (Gps.Gps.CurrentLatLon.Heading > headingToWaypoint - 5 && Gps.Gps.CurrentLatLon.Heading < headingToWaypoint + 5)
                {
                    travelRotationY = 0;
                    turnDirection = "None";
                }
                else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading + 20)
                {
                    if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180)
                    {
                        turnDirection = "Left+";
                        travelRotationY = -2;
                    }
                    else
                    {
                        turnDirection = "Right+";
                        travelRotationY = 2;
                    }
                }
                else if (headingToWaypoint > Gps.Gps.CurrentLatLon.Heading)
                {
                    if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint > 180)
                    {
                        turnDirection = "Left";
                        travelRotationY = -1;
                    }
                    else
                    {
                        turnDirection = "Right";
                        travelRotationY = 1;
                    }
                }
                else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading - 20) //If it has a long ways to turn, go fast!
                {
                    if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180)
                    {
                        turnDirection = "Left+";
                        travelRotationY = -2;
                    }
                    else
                    {
                        turnDirection = "Right+";
                        travelRotationY = 2; //Turn towards its right
                    }
                }
                else if (headingToWaypoint < Gps.Gps.CurrentLatLon.Heading)
                {
                    if (Gps.Gps.CurrentLatLon.Heading - headingToWaypoint < 180)
                    {
                        turnDirection = "Left";
                        travelRotationY = -1;
                    }
                    else
                    {
                        turnDirection = "Right";
                        travelRotationY = 1;
                    }
                }

                _ikController.RequestMovement(nomGaitSpeed, travelLengthX, travelLengthZ, travelRotationY);

                await Task.Delay(50, cancelationToken);

                distanceHeading = GpsExtensions.GetDistanceAndHeadingToDestination(Gps.Gps.CurrentLatLon.Lat, Gps.Gps.CurrentLatLon.Lon, currentWaypoint.Lat, currentWaypoint.Lon);
                distanceToWaypoint = distanceHeading[0];
                headingToWaypoint = distanceHeading[1];

                if (cancelationToken.IsCancellationRequested)
                    return false;
            }

            await _display.WriteAsync($"WP D/H {distanceToWaypoint}, {headingToWaypoint}", 1);
            await _display.WriteAsync($"Heading {Gps.Gps.CurrentLatLon.Heading}", 2);

            return true;
        }
    }