Autonomous driving and obstacle avoiding Rover

using rover.sl.Geography;
using rover.sl.Motors;
using rover.sl.Sensors.Compass;
using rover.sl.Sensors.Encoder;
using rover.sl.Sensors.Rangefinder;
using System;
using System.Threading.Tasks;

namespace rover.sl.Drive
{
    public class ObstacleDriveController : DriveBase
    {
        /// <summary>
        /// Der Rover steht vor einem fernen Hindernis in Zentimetern.
        /// </summary>
        private const int ROVER_FAR_OBSTACLE_CENTIMETERS = 500; // Feintuning: Minimum: 160 | Standard: 500

        /// <summary>
        /// Der Rover steht vor einem nahmen Hindernis in Zentimetern.
        /// </summary>
        private const int ROVER_NEAR_OBSTACLE_CENTIMETERS = 150; // Feintuning: Minimum: 120 | Standard: 150

        /// <summary>
        /// Der Rover steht vor einem nahem Hindernis. Ausweichen/Lenken in Grad.
        /// </summary>
        private const int ROVER_NEAR_OBSTACLE_TURN_DEGREES = 90;

        /// <summary>
        /// Der Rover steht vor einem fernen Hindernis. Ausweichen/Lenken nach rechts in Grad.
        /// </summary>
        private const double ROVER_FAR_OBSTACLE_TURN_RIGHT_DEGREES = 3;

        /// <summary>
        /// Der Rover steht vor einem fernen Hindernis. Ausweichen/Lenken nach links in Grad.
        /// </summary>
        private const double ROVER_FAR_OBSTACLE_TURN_LEFT_DEGREES = ROVER_FAR_OBSTACLE_TURN_RIGHT_DEGREES * -1;

        /// <summary>
        /// Anteil der Strecke die in eine Richtung leicht ausgewichen werden soll bei einem fernen Hindernis bevor in die andere Richtung leicht ausgewichen werden soll.
        /// </summary>
        private const int THIRDS_TIME_TO_OBSTACLE_DIVIDER = 6; // Feintuning: Minimum: 5d | Standard: 8d

        /// <summary>
        /// Start der Fahrzeit nachdem ein Weg ohne Hindernisse gefunden wurde und zuvor ein nahes oder fernes Hindernis erkannt wurde.
        /// </summary>
        private DateTime? _driveObstacleFree = null;

        /// <summary>
        /// Die Zeit die der Rover sich nach rechts oder links freifahren soll, wenn er einem fernen Hindernis ausweicht.
        /// </summary>
        private const int DRIVE_OBSTACLE_FREE_TIME_STARIGHT_AWAY = 700; // Feintuning: Minimum: 500 | Standard: 700

        /// <summary>
        /// Die Zeit die der Rover sich nach rechts oder links freifahren soll, wenn er einem nahen Hindernis ausweicht.
        /// </summary>
        private const int DRIVE_OBSTACLE_FREE_TIME_NOT_STARIGHT_AWAY = 1500; // Feintuning: Minimum: 2000 | Standard: 1500

        /// <summary>
        /// Die Zeit die der Rover sich nach rechts oder links freifahren soll und bereits in eine Richtung gefahren ist.
        /// </summary>
        private const int DRIVE_OBSTACLE_FREE_TIME_NOT_STARIGHT_AWAY_DOUBLE_TIME_FACTOR = 2600; // Feintuning: Minimum: 3000 | Standard: 2600

        /// <summary>
        /// Faktor wenn der Rover bereits ausgerichtet wurde und sich vollständig in die endgegengesetzte Richtung wenden muss.
        /// </summary>
        private const int AVOID_OBSTACLE_BEARING_DEGREES_REVERT = 2;

        // Abhänigkeiten.
        private RangefinderController _rangefinderController;

        public ObstacleDriveController(MotorController motorController, EncoderController encoderController, RangefinderController rangefinderController)
            : base(motorController, encoderController)
        {
            _rangefinderController = rangefinderController;
        }

        /// <summary>
        /// Erkennt und umfährt Hindernisse.
        /// </summary>
        /// <param name="roverSpeed">Geschwindigkeit in km/h.</param>
        /// <param name="markerId">Marker Id.</param>
        /// <param name="defaultLeftOrRight">Die Standard Richtung in die der Rover länger ausweichen soll. True = links. False = rechts.</param>
        /// <returns></returns>
        public async Task<ObstacleDriveResult> AvoidObstaclesAsync(double roverSpeed, long markerId, bool defaultLeftOrRight)
        {
            // Hindernis umfahren.
            var obstacleDetectedAndCancel = false;

            // Entfernung zu nächstem davorliegenden Hindernis.
            var obstacleDistanceCentimeter = _rangefinderController.FrontDistanceCentimeters;

            // Ein nahes Hindernis wurde erkannt.
            var nearObstacle = obstacleDistanceCentimeter <= ROVER_NEAR_OBSTACLE_CENTIMETERS;

            // Ein fernes Hindernis wurde erkannt.
            var farObstacle = obstacleDistanceCentimeter <= ROVER_FAR_OBSTACLE_CENTIMETERS;

            // Ein Hindernis wurde erkannt.
            var anyObstacleDetected = nearObstacle || farObstacle;

            // Die aktuelle Ausrichtung des Rovers wenn er versucht Hindernissen auszuweichen.
            var obstacleAvoidDirection = new ObstacleAvoidDirection();
            obstacleAvoidDirection.LeftOrRight = defaultLeftOrRight;

            // Die neue Standard Richtung in die der Rover länger ausweichen soll. True = links.False = rechts.
            var newDefaultLeftOrRight = defaultLeftOrRight;

            // Gibt an ob gefahren werden muss um dem Hindernis auszuweichen.
            var driveObstacleAvoid = false;

            // Gibt an wie lange der Rover fahren muss um das Hindernis zu erreichen.
            var timeToObstacle = GetTimeToDistance(roverSpeed, obstacleDistanceCentimeter);

            // Gibt an wie lange der Rover zum drittel des Ziels fahren muss.
            var thirdsTimeToObstacle = timeToObstacle / THIRDS_TIME_TO_OBSTACLE_DIVIDER;

            // Ein Hindernis wurde erkannt oder es wurde kein Hindernis mehr erkannt und für eine angegebene Zeit in diese Richtung fahren.
            while (anyObstacleDetected || driveObstacleAvoid)
            {
                // Abbruch.
                if (await CancelRequiredAsync(markerId, true))
                {
                    // Abbruch.
                    obstacleDetectedAndCancel = true;
                    break;
                }

                // Hindernis wurde erkannt.
                obstacleDetectedAndCancel = true;

                // Wenn kein Hindernis mehr erkannt wurde.
                if (anyObstacleDetected == false)
                {
                    // Starte Fahrzeit wenn kein Hindernis mehr erkannt wurde und zuvor ein nahes oder fernes Hindernis erkannt wurde.
                    if (_driveObstacleFree.HasValue == false)
                    {
                        _driveObstacleFree = DateTime.Now;
                    }

                    // Der Rover steht gerade, da er einem fernen Hindernis ausweicht.
                    if (obstacleAvoidDirection.IsStraightAway)
                    {
                        // Rover muss mit einer leichten Kurve freigefahren werden. Übergebe Motor Befehl.
                        await MoveRoverAsync(obstacleAvoidDirection.DriveFarObstacleFreeTurnDegrees, roverSpeed);
                    }
                    // Der Rover steht seitwärts, da er einem nahen Hindernis ausweicht.
                    else
                    {
                        // Rover muss gerade aus freigefahren werden. Übergebe Motor Befehl.
                        await MoveRoverAsync(roverSpeed);
                    }
                }
                // Wenn ein Hindernis erkannt wurde.
                else
                {
                    // Wenn der Rover vor einem nahen Hindernis steht und sich wenden möchte muss er zuerst gestoppt werden.
                    if (nearObstacle && _encoderController.IsStopped == false)
                    {
                        // Abbruch.
                        if (await CancelRequiredAsync(markerId, true))
                        {
                            // Abbruch.
                            obstacleDetectedAndCancel = true;
                            break;
                        }

                        // Übergebe Motor Stop Befehl.
                        await StopRoverAsync();
                    }
                    // Der Rover steht vor einem nahmen Hindernis.
                    else if (nearObstacle)
                    {
                        // Setze Start der Fahrzeit zurück wenn ein Hindernis erkannt wurde.
                        _driveObstacleFree = null;

                        // Wenn bereits versucht wurde nach rechts und links auszuweichen, fahre nach hinten.
                        if (obstacleAvoidDirection.IsStraightAway == false && obstacleAvoidDirection.LeftOrRight == defaultLeftOrRight)
                        {
                            // Richte den Rover gerade aus.
                            if (await TurnRoverStraightAwayAsync(obstacleAvoidDirection, markerId))
                            {
                                // Abbruch.
                                obstacleDetectedAndCancel = true;
                                break;
                            }

                            // Fahre entsprechend der angegebenen Zeit und wenn es kein Hindernis gibt zurück.
                            if (await DriveBackAsync(roverSpeed, DRIVE_OBSTACLE_FREE_TIME_NOT_STARIGHT_AWAY, markerId))
                            {
                                // Abbruch.
                                obstacleDetectedAndCancel = true;
                                break;
                            }

                            // Rover ist gerade ausgerichtet.
                            obstacleAvoidDirection.IsStraightAway = true;

                            // Neue Standardausweich Richtung setzen.
                            newDefaultLeftOrRight = obstacleAvoidDirection.LeftOrRight;

                            // Abbruch. Der Rover konnte sich nicht freifahren. Führt zur Aktualisierung der Navigationsdaten. Ggf. ist der Rover dann nicht mehr blockiert.
                            obstacleDetectedAndCancel = true;
                            break;
                        }
                        // Wenn der Rover noch nicht versucht hat nach links und rechts auszuweichen.
                        else
                        {
                            // Die neue Standard Richtung in die der Rover länger ausweichen soll. True = links.False = rechts.
                            newDefaultLeftOrRight = obstacleAvoidDirection.LeftOrRight;

                            // Setze nächste Richtung in die der Rover ausweichen soll.
                            obstacleAvoidDirection.LeftOrRight = obstacleAvoidDirection.LeftOrRight == false;

                            // Der Rover versucht dem Hindernis links auszuweichen.
                            var avoidObstacleBearingDegrees = -ROVER_NEAR_OBSTACLE_TURN_DEGREES;

                            // Der Rover versucht dem Hindernis rechts auszuweichen.
                            if (obstacleAvoidDirection.LeftOrRight == false)
                            {
                                avoidObstacleBearingDegrees = ROVER_NEAR_OBSTACLE_TURN_DEGREES;
                            }

                            // Der Rover wurde bereits ausgerichtet und muss sich vollständig in die endgegengesetzte Richtung wenden.
                            if (obstacleAvoidDirection.IsStraightAway == false)
                            {
                                avoidObstacleBearingDegrees *= AVOID_OBSTACLE_BEARING_DEGREES_REVERT;
                            }

                            // Winkel in dem der Rover dem Hindernis ausweicht.
                            var avoidObstacleBearing = GeographyCalculator.CalculateBearing(CompassController.Heading, avoidObstacleBearingDegrees);

                            // Wende in andere Richtung als dem Hindernis.
                            if (await TurnRoverToBearingAsync(avoidObstacleBearing, markerId, true))
                            {
                                // Abbruch.
                                obstacleDetectedAndCancel = true;
                                break;
                            }

                            // Rover fährt gerade aus.
                            await MoveRoverAsync(roverSpeed);

                            // Rover muss freigefahren werden.
                            obstacleAvoidDirection.DriveObstacleAvoid = true;

                            // Rover ist nicht gerade ausgerichtet.
                            obstacleAvoidDirection.IsStraightAway = false;
                        }
                    }
                    // Der Rover steht vor einem fernen Hindernis und ist gerade ausgerichtet.
                    else if (obstacleAvoidDirection.IsStraightAway)
                    {
                        // Abbruch.
                        if (await CancelRequiredAsync(markerId, true))
                        {
                            obstacleDetectedAndCancel = true;
                            break;
                        }

                        // Setze Start der Fahrzeit zurück wenn ein Hindernis erkannt wurde.
                        _driveObstacleFree = null;

                        // Wenn noch nicht versucht wurde einem fernen Hindernis eine bestimmte Zeit auszuweichen.
                        if (obstacleAvoidDirection.FarObstacleDirectionTime.HasValue == false)
                        {
                            // Setze nächste Richtung in die der Rover ausweichen soll.
                            obstacleAvoidDirection.LeftOrRight = obstacleAvoidDirection.LeftOrRight == false;

                            // Starte die Zeit in der der Rover versucht einem Hindernis auszuweichen.
                            obstacleAvoidDirection.FarObstacleDirectionTime = DateTime.Now;
                        }
                        // Wenn der Rover eine bestimmte Zeit versucht hat einem Hindernis in eine Richtung auszuweichen in die andere Richtung wechseln.
                        else if (DateTime.Now >= obstacleAvoidDirection.FarObstacleDirectionTime.Value.AddMilliseconds(thirdsTimeToObstacle))
                        {
                            // Wenn bereits versucht wurde nach rechts und links auszuweichen. Abbruch.
                            if (obstacleAvoidDirection.LeftOrRight == defaultLeftOrRight)
                            {
                                // Die neue Standard Richtung in die der Rover länger ausweichen soll. True = links.False = rechts.
                                newDefaultLeftOrRight = obstacleAvoidDirection.LeftOrRight;

                                // Abbruch. Der Rover konnte sich nicht freifahren. Führt zur Aktualisierung der Navigationsdaten. Ggf. ist der Rover dann nicht mehr blockiert.
                                obstacleDetectedAndCancel = true;
                                break;
                            }

                            // Setze nächste Richtung in die der Rover ausweichen soll.
                            obstacleAvoidDirection.LeftOrRight = obstacleAvoidDirection.LeftOrRight == false;

                            // Starte die Zeit in der der Rover versucht einem Hindernis auszuweichen.
                            obstacleAvoidDirection.FarObstacleDirectionTime = DateTime.Now;
                        }

                        // Die neue Standard Richtung in die der Rover länger ausweichen soll. True = links.False = rechts.
                        newDefaultLeftOrRight = obstacleAvoidDirection.LeftOrRight;

                        // Der Rover versucht dem fernen Hindernis links auszuweichen.
                        if (obstacleAvoidDirection.LeftOrRight == true)
                        {
                            // Übergebe den Motor Befehl nach links ausweichen.
                            await MoveRoverAsync(ROVER_FAR_OBSTACLE_TURN_LEFT_DEGREES, roverSpeed);

                            // Gibt an mit wieviel Grad der Rover nach links sich freifahren soll.
                            obstacleAvoidDirection.DriveFarObstacleFreeTurnDegrees = ROVER_FAR_OBSTACLE_TURN_LEFT_DEGREES;
                        }
                        // Der Rover versucht dem fernen Hindernis rechts auszuweichen.
                        else
                        {
                            // Übergebe den Motor Befehl nach rechts ausweichen.
                            await MoveRoverAsync(ROVER_FAR_OBSTACLE_TURN_RIGHT_DEGREES, roverSpeed);

                            // Gibt an mit wieviel Grad der Rover nach links sich freifahren soll.
                            obstacleAvoidDirection.DriveFarObstacleFreeTurnDegrees = ROVER_FAR_OBSTACLE_TURN_RIGHT_DEGREES;
                        }

                        // Rover muss freigefahren werden.
                        obstacleAvoidDirection.DriveObstacleAvoid = true;
                    }
                }

                // Wartezeit bis die Berechnung der autonomen Fahrt erneut ausgeführt wird.
                await Task.Delay(DRIVE_LOOP_TIMEOUT_MS);

                // Entfernung zu nächstem davorliegenden Hindernis.
                obstacleDistanceCentimeter = _rangefinderController.FrontDistanceCentimeters;

                // Prüfen ob weiterhin ein nahes Hindernis erkennt wird.
                nearObstacle = obstacleDistanceCentimeter <= ROVER_NEAR_OBSTACLE_CENTIMETERS;

                // Prüfen ob weiterhin ein fernes Hindernis erkennt wird.
                farObstacle = obstacleDistanceCentimeter <= ROVER_FAR_OBSTACLE_CENTIMETERS && obstacleAvoidDirection.IsStraightAway;

                // Ein Hinderniss wurde erkannt.
                anyObstacleDetected = nearObstacle || farObstacle;

                // Gibt an ob der Rover sich seitwärts wegen einem nahmen Hindernis freifahren muss, oder sich wegen einem fernen Hindernis freifahren muss
                // um diesem nicht zu knapp auszuweichen. Der Rover muss sich freifahren wenn er nicht blockiert war und seitwärts fährt, oder mit leichter
                // Kurve einem fernen Hindernis ausweicht.
                driveObstacleAvoid = obstacleAvoidDirection.DriveObstacleAvoid;

                // Der Rover muss sich freifahren.
                if (driveObstacleAvoid)
                {
                    // Die Zeit die der Rover sich seitwärts freifahren soll, nach einem nahen Hindernis.
                    var driveObstacleFreeTime = DRIVE_OBSTACLE_FREE_TIME_NOT_STARIGHT_AWAY;

                    // Der Rover steht gerade, da er einem fernen Hindernis ausweicht.
                    if (obstacleAvoidDirection.IsStraightAway)
                    {
                        // Die Zeit die der Rover sich freifahren soll, nach einem fernen Hindernis.
                        driveObstacleFreeTime = DRIVE_OBSTACLE_FREE_TIME_STARIGHT_AWAY;
                    }
                    // Der Rover steht seitwärts, da er einem nahen Hindernis ausweicht.
                    else
                    {
                        // Wenn der Rover bereits in die eine Richtung ausgewichen ist und nun in die andere Richtung ausweicht muss er doppel solange fahren,
                        // da er erst die andere Ausweichstrecke zurückfahren muss.
                        if (obstacleAvoidDirection.LeftOrRight == defaultLeftOrRight)
                        {
                            driveObstacleFreeTime = DRIVE_OBSTACLE_FREE_TIME_NOT_STARIGHT_AWAY_DOUBLE_TIME_FACTOR;
                        }
                    }

                    // Gibt an ob gefahren werden muss um einem nahen Hindernis seitwärts auszuweichen oder einem fernen Hindernis mit einer leichten Kurve.
                    driveObstacleAvoid = _driveObstacleFree.HasValue == false || DateTime.Now <= _driveObstacleFree.Value.AddMilliseconds(driveObstacleFreeTime);
                }
            }

            // Wenn Rover nicht gerade ausgerichtet ist.
            if (obstacleAvoidDirection.IsStraightAway == false)
            {
                // Der Rover muss angehalten werden für eine direkte Drehung. 
                if (await DriveStopAsync(markerId))
                {
                    // Abbruch.
                    obstacleDetectedAndCancel = true;
                }
                else
                {
                    // Richte den Rover gerade aus.
                    if (await TurnRoverStraightAwayAsync(obstacleAvoidDirection, markerId))
                    {
                        // Abbruch.
                        obstacleDetectedAndCancel = true;
                    }
                }
            }

            // Ergebnis der Hindernissumfahrung.
            var obstacleAvoidResult = new ObstacleDriveResult
            {
                Cancel = obstacleDetectedAndCancel,
                DefaultLeftOrRight = newDefaultLeftOrRight
            };

            return obstacleAvoidResult;
        }

        /// <summary>
        /// Halte den Rover an.
        /// </summary>
        /// <param name="markerId">Marker Id.</param>
        /// <returns></returns>
        private async Task<bool> DriveStopAsync(long markerId)
        {
            // Rover anhalten.
            while (_encoderController.IsStopped == false)
            {
                // Abbruch.
                if (await CancelRequiredAsync(markerId, true))
                {
                    return true;
                }

                // Übergebe Motor Stop Befehl.
                await StopRoverAsync();

                // Wartezeit bis geprüft wird ob der Rover gestoppet ist.
                await Task.Delay(DRIVE_STOP_CHECK_MS);
            }

            return false;
        }

        private async Task<bool> DriveBackAsync(double roverSpeed, int driveBackTime, long markerId)
        {
            // Entfernung zu nächstem dahinterliegenden Hindernis.
            var backObstacleDistanceCentimeter = _rangefinderController.BackDistanceCentimeters;

            // Ein nahes dahinterliegendes Hindernis wurde erkannt.
            var backNearObstacle = backObstacleDistanceCentimeter <= ROVER_NEAR_OBSTACLE_CENTIMETERS;

            // Rover fährt eine angegebene Zeit Rückwärts, oder solange bis Hindernisse erkannt werden.
            var driveBackStart = DateTime.Now;
            while (backNearObstacle == false && DateTime.Now <= driveBackStart.AddMilliseconds(driveBackTime))
            {
                // Abbruch.
                if (await CancelRequiredAsync(markerId, true))
                {
                    return true;
                }

                // Rover fährt rückwärts.
                await MoveRoverAsync(roverSpeed, false);

                // Wartezeit bis geprüft wird ob der Rover weiter Rückwärts fahren muss.
                await Task.Delay(DRIVE_BACK_CHECK_MS);

                // Entfernung zu nächstem dahinterliegenden Hindernis.
                backObstacleDistanceCentimeter = _rangefinderController.BackDistanceCentimeters;

                // Ein nahes dahinterliegendes Hindernis wurde erkannt.
                backNearObstacle = backObstacleDistanceCentimeter <= ROVER_NEAR_OBSTACLE_CENTIMETERS;
            }

            // Abbruch.
            if (await CancelRequiredAsync(markerId, true))
            {
                return true;
            }

            // Übergebe Motor Stop Befehl.
            await StopRoverAsync();

            return false;
        }

        /// <summary>
        /// Rover gerade ausrichten.
        /// </summary>
        /// <param name="obstacleAvoidDirection">Information über die Ausrichtung des Rovers.</param>
        /// <param name="markerId">Marker Id.</param>
        /// <returns></returns>
        private async Task<bool> TurnRoverStraightAwayAsync(ObstacleAvoidDirection obstacleAvoidDirection, long markerId)
        {
            // Wenn der Rover nicht gerade ausgerichtet ist.
            if (obstacleAvoidDirection.IsStraightAway == false)
            {
                // Der Rover muss gerade ausgerichtet.
                var avoidObstacleBearingDegrees = obstacleAvoidDirection.LeftOrRight ? ROVER_NEAR_OBSTACLE_TURN_DEGREES : -ROVER_NEAR_OBSTACLE_TURN_DEGREES;

                // Winkel in dem der Rover sich gerade ausrichtet.
                var avoidObstacleBearing = GeographyCalculator.CalculateBearing(CompassController.Heading, avoidObstacleBearingDegrees);

                // Richte Rover gerade aus.
                if (await TurnRoverToBearingAsync(avoidObstacleBearing, markerId, true))
                {
                    // Abbruch.
                    return true;
                }
            }

            return false;
        }
    }
}