From be091a57841a46fe5f0e0d519fb368cb1cb17730 Mon Sep 17 00:00:00 2001
From: Marc Riera <marc@marcriera.cat>
Date: Sun, 29 Dec 2024 01:40:38 +0100
Subject: [PATCH] ATC Dimetronic: ATO notch delay

---
 src/Devices/AtcDimetronic.cs | 36 ++++++++++++++++++++++++++++++++----
 1 file changed, 32 insertions(+), 4 deletions(-)

diff --git a/src/Devices/AtcDimetronic.cs b/src/Devices/AtcDimetronic.cs
index f5490a2..32428b9 100644
--- a/src/Devices/AtcDimetronic.cs
+++ b/src/Devices/AtcDimetronic.cs
@@ -83,6 +83,12 @@ namespace OpenbveFcmbTrainPlugin
         /// <summary>Whether ATO is available or not on the train.</summary>
         private readonly bool AtoAvailable;
 
+        /// <summary>The delay before ATO changes notches.</summary>
+        private readonly double AtoNotchDelay = 500;
+
+        /// <summary>The counter for the ATO notch delay.</summary>
+        private double AtoNotchDelayCounter;
+
         /// <summary>The current position of the train.</summary>
         private double TrainLocation;
 
@@ -383,7 +389,7 @@ namespace OpenbveFcmbTrainPlugin
                     train.ContinuousProtection = true;
                     train.VigilanceOverride = true;
                     CurrentSpeedCode = GetSpeedCodeFromAspect();
-                    ProcessAto(train, route);
+                    ProcessAto(train, route, elapsedTime);
                     // If reverser is not forward or emergency brake is applied, unselect mode
                     if (train.PhysicalHandles.Reverser != 1 || train.PhysicalHandles.BrakeNotch > train.Specs.BrakeNotches)
                     {
@@ -691,7 +697,7 @@ namespace OpenbveFcmbTrainPlugin
         }
 
         /// <summary>Processes ATO orders.</summary>
-        private void ProcessAto(Train train, Route route)
+        private void ProcessAto(Train train, Route route, Time elapsedTime)
         {
             double speed = train.State.Speed.KilometersPerHour;
             bool stopped = speed < 0.05;
@@ -700,6 +706,9 @@ namespace OpenbveFcmbTrainPlugin
             Speed target = CurrentSpeedCode.TargetLimit;
             double distance = CurrentSpeedCode.TargetPosition - TrainLocation;
 
+            // Update notch delay counter
+            AtoNotchDelayCounter += elapsedTime.Milliseconds;
+
             // Calculate notches according to state
             switch (AtoState)
             {
@@ -724,10 +733,29 @@ namespace OpenbveFcmbTrainPlugin
                     break;
                 case AtoStates.IncreaseSpeed:
                     AtoBrakeNotch = 0;
-                    if (train.Acceleration < 0.2 && AtoPowerNotch < train.Specs.PowerNotches)
+                    if (train.Acceleration < 0.2 && AtoPowerNotch < train.Specs.PowerNotches && AtoNotchDelayCounter >= AtoNotchDelay)
                     {
-                        // TODO: Add delay
                         AtoPowerNotch++;
+                        AtoNotchDelayCounter = 0;
+                    }
+                    if (train.State.Speed.KilometersPerHour >= target.KilometersPerHour - 5)
+                    {
+                        AtoState = AtoStates.MaintainSpeed;
+                    }
+                    break;
+                case AtoStates.MaintainSpeed:
+                    AtoBrakeNotch = 0;
+                    if (train.Acceleration > 0 && AtoNotchDelayCounter >= AtoNotchDelay)
+                    {
+                        if (AtoPowerNotch > 0)
+                        {
+                            AtoPowerNotch--;
+                        }
+                        else if (AtoBrakeNotch < train.Specs.BrakeNotches)
+                        {
+                            AtoBrakeNotch++;
+                        }
+                        AtoNotchDelayCounter = 0;
                     }
                     break;
             }