From 426b27f0dd01f800dc0cb31cf71782bbb7cb85f9 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Thu, 30 Sep 2021 22:55:14 +0200
Subject: [PATCH] Reworked use of ringbuffer

---
 RingStream.cpp | 15 ++++++++++++---
 RingStream.h   |  7 +++++--
 WifiESP.cpp    | 49 +++++++++++++++++++++++++++++++++++++++++--------
 3 files changed, 58 insertions(+), 13 deletions(-)

diff --git a/RingStream.cpp b/RingStream.cpp
index 2c28d1b..baa4cc1 100644
--- a/RingStream.cpp
+++ b/RingStream.cpp
@@ -45,10 +45,11 @@ size_t RingStream::write(uint8_t b) {
   return 1;
 }
 
-int RingStream::read() {
-  if ((_pos_read==_pos_write) && !_overflow) return -1;  // empty  
+int RingStream::read(byte advance) {
+  if ((_pos_read==_pos_write) && !_overflow) return -1;  // empty
+  if (_pos_read == _mark) return -1;
   byte b=_buffer[_pos_read];
-  _pos_read++;
+  _pos_read += advance;
   if (_pos_read==_len) _pos_read=0;
   _overflow=false;
   return b;
@@ -68,6 +69,7 @@ int RingStream::freeSpace() {
 
 // mark start of message with client id (0...9)
 void RingStream::mark(uint8_t b) {
+    //DIAG(F("Mark1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
     _mark=_pos_write;
     write(b); // client id
     write((uint8_t)0);  // count MSB placemarker
@@ -81,7 +83,12 @@ uint8_t RingStream::peekTargetMark() {
   return _buffer[_mark];
 }
 
+void RingStream::info() {
+  DIAG(F("Info len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
+}
+
 bool RingStream::commit() {
+  //DIAG(F("Commit1 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
   if (_overflow) {
         DIAG(F("RingStream(%d) commit(%d) OVERFLOW"),_len, _count);
         // just throw it away 
@@ -101,5 +108,7 @@ bool RingStream::commit() {
   _mark++;
   if (_mark==_len) _mark=0;
   _buffer[_mark]=lowByte(_count);
+  _mark=_len+1;
+  //DIAG(F("Commit2 len=%d count=%d pr=%d pw=%d m=%d"),_len, _count,_pos_read,_pos_write,_mark);
   return true; // commit worked
 }
diff --git a/RingStream.h b/RingStream.h
index 790c66e..974a235 100644
--- a/RingStream.h
+++ b/RingStream.h
@@ -28,14 +28,17 @@ class RingStream : public Print {
   
     virtual size_t write(uint8_t b);
     using Print::write;
-    int read();
+    inline int read() { return read(1); };
+    inline int peek() { return read(0); };
     int count();
     int freeSpace();
     void mark(uint8_t b);
     bool commit();
     uint8_t peekTargetMark();
-    
+    void info();
+
  private:
+   int read(byte advance);
    int _len;
    int _pos_write;
    int _pos_read;
diff --git a/WifiESP.cpp b/WifiESP.cpp
index b46a13d..8948e3d 100644
--- a/WifiESP.cpp
+++ b/WifiESP.cpp
@@ -52,18 +52,22 @@ static void handleData(void* arg, AsyncClient* client, void *data, size_t len) {
   }
 }
 
-bool sendData(uint8_t clientId, char* data, int count) {
-  AsyncClient *client = clients[clientId];
+//static AsyncClient *debugclient = NULL;
+
+bool sendData(AsyncClient *client, char* data, size_t count) {
   size_t willsend = 0;
 
   // reply to client
   if (client->canSend()) {
     while (count > 0) {
-      willsend = client->add(data, count); // add checks for space()
+      if (client->connected())
+	willsend = client->add(data, count); // add checks for space()
+      else
+	willsend = 0;
       if (willsend < count) {
 	DIAG(F("Willsend %d of count %d"), willsend, count);
       }
-      if (client->send()) {
+      if (client->connected() && client->send()) {
 	count = count - willsend;
 	data = data + willsend;
       } else {
@@ -78,12 +82,23 @@ bool sendData(uint8_t clientId, char* data, int count) {
   return false;
 }
 
+static void deleteClient(AsyncClient* client) {
+  uint8_t clientId;
+  for (clientId=0; clientId<clients.size(); clientId++){
+    if (clients[clientId] == client) break;
+  }
+  if (clientId < clients.size()) {
+    clients[clientId] = NULL;
+  }
+}
 static void handleDisconnect(void* arg, AsyncClient* client) {
   DIAG(F("client %s disconnected"), client->remoteIP().toString().c_str());
+  deleteClient(client);
 }
 
 static void handleTimeOut(void* arg, AsyncClient* client, uint32_t time) {
   DIAG(F("client ACK timeout ip: %s"), client->remoteIP().toString().c_str());
+  deleteClient(client);
 }
 
 
@@ -154,11 +169,27 @@ bool WifiESP::setup(const char *wifiESSID,
 }
 
 void WifiESP::loop() {
-
+  AsyncClient *client = NULL;
   // Do something with outboundRing
   // call sendData
-  int clientId=outboundRing->read();
-  if (clientId>=0) {
+  int clientId=outboundRing->peek();
+  if (clientId >= 0) {
+    if (clientId > clients.size()) {
+      // something is wrong with the ringbuffer position
+      outboundRing->info();
+      client = NULL;
+    } else {
+      client = clients[clientId];
+    }
+//    if (client != debugclient) {
+//      DIAG(F("new client pointer = %x from id %d"), client, clientId);
+//      debugclient = client;
+//    }
+  } else {
+    client = NULL;
+  }
+  if (clientId>=0 && client && client->connected() && client->canSend()) {
+    outboundRing->read();
     int count=outboundRing->count();
     //DIAG(F("Wifi reply client=%d, count=%d"), clientId,count);
     {
@@ -174,9 +205,11 @@ void WifiESP::loop() {
       }
       buffer[count]=0;
       //DIAG(F("SEND:%s COUNT:%d"),buffer,count);
-      while (! sendData(clientId, buffer, count)) {
+      uint8_t tries = 3;
+      while (! sendData(client, buffer, count)) {
 	DIAG(F("senData fail"));
 	yield();
+	if (tries == 0) break;
       }
     }
   }