From 93446f19eda0ca88b26ab8f92792229296f6bcb5 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Sat, 1 Aug 2020 14:06:39 +0200
Subject: [PATCH] Two functions changeFn and setFn dependent on if called from
 WiThrottle or DCCEXparser

---
 DCC.cpp        | 53 +++++++++++++++++++++++++++++++++++++++++---------
 DCC.h          |  4 +++-
 WiThrottle.cpp |  8 ++++++--
 3 files changed, 53 insertions(+), 12 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index c4c167f..8b70e5d 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -95,32 +95,67 @@ bool DCC::getThrottleDirection(int cab) {
   return (speedTable[reg].speedCode & 0x80) !=0;
 }
 
-void DCC::setFn( int cab, byte functionNumber, bool pressed) {
+void DCC::setFn( int cab, byte functionNumber, bool on) {
   if (cab<=0 || functionNumber>28) return;
   int reg = lookupSpeedTable(cab);
   if (reg<0) return;  
 
+  // Take care of functions:
+  // Set state of function
+  unsigned long funcmask = (1L<<functionNumber);
+  if (on) {
+      speedTable[reg].functions |= funcmask;
+  } else {
+      speedTable[reg].functions &= ~funcmask;
+  }
+  //DIAG(F("\nFUNCTIONS of %d IS %d\n"),cab,speedTable[reg].functions);
+  updateGroupflags(speedTable[reg].groupFlags, functionNumber);
+  return;
+}
+
+// Change function according to how button was pressed,
+// typically in WiThrottle.
+// Returns new state or -1 if nothing was changed.
+int DCC::changeFn( int cab, byte functionNumber, bool pressed) {
+  int funcstate = -1;
+  if (cab<=0 || functionNumber>28) return funcstate;
+  int reg = lookupSpeedTable(cab);
+  if (reg<0) return funcstate;  
+
   // Take care of functions:
   // Imitate how many command stations do it: Button press is
   // toggle but for F2 where it is momentary
+  unsigned long funcmask = (1L<<functionNumber);
   if (functionNumber == 2) {
       // turn on F2 on press and off again at release of button
-      if (pressed) speedTable[reg].functions |= (1L<<functionNumber);
-      else speedTable[reg].functions &= ~(1L<<functionNumber);
+      if (pressed) {
+	  speedTable[reg].functions |= funcmask;
+	  funcstate = 1;
+      } else {
+	  speedTable[reg].functions &= ~funcmask;
+	  funcstate = 0;
+      }
   } else {
       // toggle function on release, ignore release
-      if (pressed)
-	  speedTable[reg].functions ^= (1UL<<functionNumber);
+      if (pressed) {
+	  speedTable[reg].functions ^= funcmask;
+      }
+      funcstate = speedTable[reg].functions & funcmask;
   }
-  // Set the group flag to say we have touched the particular group.
-  // A group will be reminded only if it has been touched.  
+  updateGroupflags(speedTable[reg].groupFlags, functionNumber);
+  return funcstate;
+}
+
+// Set the group flag to say we have touched the particular group.
+// A group will be reminded only if it has been touched.  
+void DCC::updateGroupflags(byte & flags, int functionNumber) {
   byte groupMask;
   if (functionNumber<=4)       groupMask=FN_GROUP_1;
   else if (functionNumber<=8)  groupMask=FN_GROUP_2;
   else if (functionNumber<=12) groupMask=FN_GROUP_3;
   else if (functionNumber<=20) groupMask=FN_GROUP_4;
   else                         groupMask=FN_GROUP_5;
-  speedTable[reg].groupFlags |= groupMask; 
+  flags |= groupMask; 
 }
 
 void DCC::setAccessory(int address, byte number, bool activate) {
@@ -342,7 +377,7 @@ void DCC::issueReminders() {
 }
  
 bool DCC::issueReminder(int reg) {
-  long functions=speedTable[reg].functions;
+  unsigned long functions=speedTable[reg].functions;
   int loco=speedTable[reg].loco;
   byte flags=speedTable[reg].groupFlags;
   
diff --git a/DCC.h b/DCC.h
index 9b15c27..d549490 100644
--- a/DCC.h
+++ b/DCC.h
@@ -60,6 +60,8 @@ class DCC {
   static void writeCVBitMain(int cab, int cv, byte bNum, bool bValue);
   static void setFunction( int cab, byte fByte, byte eByte);
   static void setFn( int cab, byte functionNumber, bool on);
+  static int  changeFn( int cab, byte functionNumber, bool pressed);
+  static void updateGroupflags(byte & flags, int functionNumber);
   static void setAccessory(int aAdd, byte aNum, bool activate) ;
   static bool writeTextPacket( byte *b, int nBytes);
   static void setDebug(bool on);
@@ -82,7 +84,7 @@ private:
      int loco;
      byte speedCode;
      byte groupFlags;
-     long functions;
+     unsigned long functions;
   };
   static byte loopStatus;
   static void setThrottle2( uint16_t cab, uint8_t speedCode);
diff --git a/WiThrottle.cpp b/WiThrottle.cpp
index dfe50ac..fac390b 100644
--- a/WiThrottle.cpp
+++ b/WiThrottle.cpp
@@ -238,11 +238,15 @@ void WiThrottle::locoAction(Print & stream, byte* aval, char throttleChar, int c
             break;
            case 'F': //F onOff function
                 {
+		  bool funcstate;
                   bool pressed=aval[1]=='1';
                   int fKey = getInt(aval+2);
                   LOOPLOCOS(throttleChar, cab) {
-                          DCC::setFn(myLocos[loco].cab, fKey, pressed);
-                   } 
+		      funcstate = DCC::changeFn(myLocos[loco].cab, fKey, pressed);
+		      if(funcstate==0 || funcstate==1)
+			  StringFormatter::send(stream,F("M%cA%c%d<;>F%d%d\n"), throttleChar, LorS(myLocos[loco].cab), 
+						myLocos[loco].cab, funcstate, fKey);
+		  }
                 }
                 break;  
             case 'q':