From 4aceff8b7c640f9471c6492893a0284f5a11eeb5 Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Sun, 5 Jul 2020 21:00:27 +0100
Subject: [PATCH] WiThrott;e FIXES

---
 DCC.cpp           |  2 +-
 WiThrottle.cpp    | 52 +++++++++++++++++++++++++++++++++--------------
 WiThrottle.h      |  5 +++--
 WifiInterface.cpp |  4 ++--
 4 files changed, 43 insertions(+), 20 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index 7051d35..462b5c4 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -390,7 +390,7 @@ int DCC::lookupSpeedTable(int locoId) {
   }
   if (reg==firstEmpty){
         speedTable[reg].loco = locoId;
-        speedTable[reg].speedCode=0;
+        speedTable[reg].speedCode=128;  // default direction forward
         speedTable[reg].groupFlags=0;
         speedTable[reg].functions=0;
   }
diff --git a/WiThrottle.cpp b/WiThrottle.cpp
index 7af7373..9aad625 100644
--- a/WiThrottle.cpp
+++ b/WiThrottle.cpp
@@ -51,27 +51,22 @@
 
 WiThrottle * WiThrottle::firstThrottle=NULL;
 
-WiThrottle* WiThrottle::getThrottle(Print & stream, int wifiClient) {
+WiThrottle* WiThrottle::getThrottle( int wifiClient) {
   for (WiThrottle* wt=firstThrottle; wt!=NULL ; wt=wt->nextThrottle)  
      if (wt->clientid==wifiClient) return wt;
-  return new WiThrottle(stream, wifiClient);
+  return new WiThrottle( wifiClient);
 }
 
  // One instance of WiTHrottle per connected client, so we know what the locos are 
  
-WiThrottle::WiThrottle(Print & stream, int wificlientid) {
+WiThrottle::WiThrottle( int wificlientid) {
    DIAG(F("\nCreating new WiThrottle for client %d\n"),wificlientid); 
    nextThrottle=firstThrottle;
    firstThrottle= this;
    clientid=wificlientid;
-   for (int loco=0;loco<MAX_MY_LOCO; loco++) myLocos[loco].throttle='\0';
-  StringFormatter::send(stream,F("VN2.0\nHTDCC++EX\nRL0\nPPA%x\nPTT]\\[Turnouts}|{Turnout]\\[Closed}|{2]\\[Thrown}|{4\\PTL"), DCCWaveform::mainTrack.getPowerMode()==POWERMODE::ON);
-
-   for(Turnout *tt=Turnout::firstTurnout;tt!=NULL;tt=tt->nextTurnout){
-        StringFormatter::send(stream,F("]\\[LT&d}|{%d}|{%d"), tt->data.id, tt->data.id, (bool)(tt->data.tStatus & STATUS_ACTIVE));
-      }
-  StringFormatter::send(stream,F("\n*10\n"));
-  heartBeatEnable=false; // until client turns it on
+    heartBeatEnable=false; // until client turns it on
+    firstCall=true;
+    for (int loco=0;loco<MAX_MY_LOCO; loco++) myLocos[loco].throttle='\0';
 }
 
 WiThrottle::~WiThrottle() {
@@ -87,10 +82,33 @@ WiThrottle::~WiThrottle() {
   }
 }
 
-void WiThrottle::parse(Print & stream, byte * cmd) {
+void WiThrottle::parse(Print & stream, byte * cmdx) {
+  
+  // we have to take a copy of the cmd buffer as the reply will get built into the cmdx  
+  byte local[50];
+  for (byte i=0;i<sizeof(local);i++) {
+    local[i]=cmdx[i];
+    if (!cmdx[i]) break;
+  }
+  local[49]='\0'; // prevent runaway parser
+  
+  byte * cmd=local;
+  
   heartBeat=millis();
   DIAG(F("\nWiThrottle(%d) [%e]"),clientid, cmd);
-  
+   
+   if (firstCall) {
+     firstCall=false;
+     StringFormatter::send(stream,F("VN2.0\nHTDCC++EX\nRL0\nPPA%x\nPTT]\\[Turnouts}|{Turnout]\\[Closed}|{2]\\[Thrown}|{4\\PTL"), 
+                DCCWaveform::mainTrack.getPowerMode()==POWERMODE::ON);
+
+    for(Turnout *tt=Turnout::firstTurnout;tt!=NULL;tt=tt->nextTurnout){
+        StringFormatter::send(stream,F("]\\[LT&d}|{%d}|{%d"), tt->data.id, tt->data.id, (bool)(tt->data.tStatus & STATUS_ACTIVE));
+    }
+    StringFormatter::send(stream,F("\n*10\n"));
+   }
+
+   while (cmd[0]) {
    switch (cmd[0]) {
        case '*':  // heartbeat control
             if (cmd[1]=='+') heartBeatEnable=true;
@@ -119,6 +137,10 @@ void WiThrottle::parse(Print & stream, byte * cmd) {
             DIAG(F("\nWiThrottle Quit"));
             delete this; 
             break;           
+   }
+   // skip over cmd until 0 or past \r or \n
+   while(*cmd !='\0' && *cmd != '\r' && *cmd !='\n') cmd++;
+   if (*cmd!='\0') cmd++; // skip \r or \n  
    }           
 }
 int WiThrottle::getInt(byte * cmd) {
@@ -196,12 +218,12 @@ void WiThrottle::locoAction(Print & stream, byte* aval, char throttleChar, int c
             case 'q':
                 if (aval[1]=='V') {   //qV
                   LOOPLOCOS(throttleChar, cab) {              
-                    StringFormatter::send(stream,F("M%cAL%d<;>V%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab));
+                    StringFormatter::send(stream,F("M%cAL%d<;>V%d\n"), throttleChar, myLocos[loco].cab, DCC::getThrottleSpeed(myLocos[loco].cab));
                   }              
                 }
                 else if (aval[1]=='R') { // qR
                   LOOPLOCOS(throttleChar, cab) {              
-                    StringFormatter::send(stream,F("M%cAL%d<;>R%d"), throttleChar, myLocos[loco].cab, DCC::getThrottleDirection(myLocos[loco].cab));
+                    StringFormatter::send(stream,F("M%cAL%d<;>R%d\n"), throttleChar, myLocos[loco].cab, DCC::getThrottleDirection(myLocos[loco].cab));
                   }             
                 }     
             break;    
diff --git a/WiThrottle.h b/WiThrottle.h
index 94cf3c0..d0c3f4b 100644
--- a/WiThrottle.h
+++ b/WiThrottle.h
@@ -29,10 +29,10 @@ class WiThrottle {
   public:  
     static void loop();
     void parse(Print & stream, byte * cmd);
-    static WiThrottle* getThrottle(Print & stream, int wifiClient); 
+    static WiThrottle* getThrottle( int wifiClient); 
 
   private: 
-    WiThrottle(Print & stream, int wifiClientId);
+    WiThrottle( int wifiClientId);
     ~WiThrottle();
    
       static const int MAX_MY_LOCO=10;
@@ -46,6 +46,7 @@ class WiThrottle {
        
       MYLOCO myLocos[MAX_MY_LOCO];   
       bool heartBeatEnable;
+      bool firstCall;
       unsigned long heartBeat;
       
      void multithrottle(Print & stream, byte * cmd);
diff --git a/WifiInterface.cpp b/WifiInterface.cpp
index 6cbefaf..1a1196c 100644
--- a/WifiInterface.cpp
+++ b/WifiInterface.cpp
@@ -172,12 +172,12 @@ void WifiInterface::loop(Stream & wifiStream) {
     }
     else if (buffer[0]=='<')  parser.parse(streamer,buffer, true);    // tell JMRI parser that callbacks are diallowed because we dont want to handle the async 
  
-    else WiThrottle::getThrottle(streamer, connectionId)->parse(streamer, buffer);
+    else WiThrottle::getThrottle(connectionId)->parse(streamer, buffer);
 
        
     if (streamer.available()) { // there is a reply to send 
         streamer.write('\0');
-        DIAG(F("WiFiInterface Responding client (%d) l(%d) %e\n"),connectionId,streamer.available()-1,buffer);
+        DIAG(F("\nWiFiInterface reply c(%d) l(%d) [%e]\n"),connectionId,streamer.available()-1,buffer);
         
         StringFormatter::send(wifiStream,F("AT+CIPSEND=%d,%d\r\n"),connectionId,streamer.available()-1);
         if (checkForOK(wifiStream,1000,PROMPT_SEARCH,true))  wifiStream.print((char *) buffer);