From 420ed7355da869bfae54116d9d300cf7e61f6197 Mon Sep 17 00:00:00 2001
From: nic547 <7825979+nic547@users.noreply.github.com>
Date: Wed, 4 May 2022 11:09:16 +0200
Subject: [PATCH] Adjust forget commands to respect the direction of engines

Instead of sending the "backwards e-stop" packet to locos the CS will send the e-stop packet with the direction the locos should already have.
---
 DCC.cpp | 31 ++++++++++++++++++++++---------
 DCC.h   |  1 +
 2 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index 8af4ccd..4db2d33 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -75,7 +75,7 @@ void DCC::begin(const FSH * motorShieldName) {
 
 
 void DCC::setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection)  {
-  byte speedCode = (tSpeed & 0x7F)  + tDirection * 128;
+  byte speedCode = calculateSpeedByte(tSpeed,tDirection);
   setThrottle2(cab, speedCode);
   TrackManager::setDCSignal(cab,speedCode); // in case this is a dcc track on this addr
   // retain speed for loco reminders
@@ -540,15 +540,24 @@ void DCC::setLocoId(int id,ACK_CALLBACK callback) {
       DCCACK::Setup(id | 0xc000,LONG_LOCO_ID_PROG, callback);
 }
 
-void DCC::forgetLoco(int cab) {  // removes any speed reminders for this loco
-  setThrottle2(cab,1); // ESTOP this loco if still on track
-  int reg=lookupSpeedTable(cab);
-  if (reg>=0) speedTable[reg].loco=0;
-  setThrottle2(cab,1); // ESTOP if this loco still on track
+void DCC::forgetLoco(int cab){ // removes any speed reminders for this loco
+  auto direction = getThrottleDirection(cab);
+  setThrottle2(cab, calculateSpeedByte(1, direction)); // ESTOP this loco if still on track
+  int reg = lookupSpeedTable(cab);
+  if (reg >= 0)
+    speedTable[reg].loco = 0;
+  setThrottle2(cab, calculateSpeedByte(1, direction)); // ESTOP if this loco still on track
 }
-void DCC::forgetAllLocos() {  // removes all speed reminders
-  setThrottle2(0,1); // ESTOP all locos still on track
-  for (int i=0;i<MAX_LOCOS;i++) speedTable[i].loco=0;
+
+void DCC::forgetAllLocos() { // removes all speed reminders
+  for (int i = 0; i < MAX_LOCOS; i++) {
+    auto &locoId = speedTable[i].loco;
+    if (locoId != 0) {
+      auto direction = getThrottleDirection(locoId);
+      setThrottle2(locoId, calculateSpeedByte(1, direction));
+      locoId = 0;
+    }
+  }
 }
 
 byte DCC::loopStatus=0;
@@ -621,6 +630,10 @@ bool DCC::issueReminder(int reg) {
 
 ///// Private helper functions below here /////////////////////
 
+inline byte DCC::calculateSpeedByte(uint8_t tSpeed, bool tDirection) {
+  return (tSpeed & 0x7F)  + tDirection * 128;
+}
+
 byte DCC::cv1(byte opcode, int cv)  {
   cv--;
   return (highByte(cv) & (byte)0x03) | opcode;
diff --git a/DCC.h b/DCC.h
index 0e4dce1..a12bb94 100644
--- a/DCC.h
+++ b/DCC.h
@@ -102,6 +102,7 @@ public:
  static byte cv2(int cv);
  
 private:
+  static byte calculateSpeedByte(uint8_t tSpeed, bool tDirection);
   static byte loopStatus;
   static void setThrottle2(uint16_t cab, uint8_t speedCode);
   static void updateLocoReminder(int loco, byte speedCode);